From 3be2266ebe8bd2f22e706914025e0e673a2db055 Mon Sep 17 00:00:00 2001 From: Yutong Wang Date: Fri, 5 Jul 2024 16:46:33 -0400 Subject: [PATCH 01/10] Add Bezier trajectory support for testing --- crazyflie/scripts/crazyflie_server.py | 98 ++++++++++++++++--- .../crazyflie_examples/bezier_test.py | 65 ++++++++++++ crazyflie_interfaces/CMakeLists.txt | 2 + .../msg/TrajectoryBezierPiece.msg | 5 + .../srv/UploadBezierTrajectory.srv | 4 + crazyflie_py/crazyflie_py/crazyflie.py | 31 ++++++ 6 files changed, 191 insertions(+), 14 deletions(-) create mode 100644 crazyflie_examples/crazyflie_examples/bezier_test.py create mode 100644 crazyflie_interfaces/msg/TrajectoryBezierPiece.msg create mode 100644 crazyflie_interfaces/srv/UploadBezierTrajectory.srv diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index ad8e299b4..d769a7650 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -19,10 +19,14 @@ import cflib.crtp from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.mem import CompressedSegment +from cflib.crazyflie.mem import CompressedStart from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.high_level_commander import HighLevelCommander from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging -from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop +from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop, UploadBezierTrajectory from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType from crazyflie_interfaces.msg import Hover from crazyflie_interfaces.msg import LogDataGeneric @@ -205,7 +209,7 @@ def __init__(self): 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() - + # Create services for the entire swarm and each individual crazyflie self.create_service(Empty, "all/emergency", self._emergency_callback) self.create_service(Takeoff, "all/takeoff", self._takeoff_callback) @@ -235,6 +239,9 @@ def __init__(self): self.create_service( UploadTrajectory, name + "/upload_trajectory", partial(self._upload_trajectory_callback, uri=uri) ) + self.create_service( + UploadBezierTrajectory, name + "/upload_bezier_trajectory", partial(self._upload_bezier_trajectory_callback, uri=uri) + ) self.create_service( NotifySetpointsStop, name + "/notify_setpoints_stop", partial(self._notify_setpoints_stop_callback, uri=uri) ) @@ -261,7 +268,7 @@ def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_ """ cf_name = self.cf_dict[link_uri] cf_type = self.type_dict[link_uri] - + logging_enabled = False logging_freq = 10 try: @@ -354,7 +361,7 @@ def _init_logging(self): if cf_handle.logging[prefix + "_logging_enabled"] and cf_handle.logging["enabled"]: callback_fnc = self.default_log_fnc[prefix] self._init_default_logging(prefix, link_uri, callback_fnc) - + # Start logging for costum logging blocks cf_handle.l_toc = cf.log.toc.toc if len(cf_handle.logging["custom_log_groups"]) != 0 and cf_handle.logging["enabled"]: @@ -410,7 +417,7 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc): except AttributeError: self.get_logger().info( f'{link_uri}: Could not add log config, bad configuration.') - + def _log_scan_data_callback(self, timestamp, data, logconf, uri): """ Once multiranger range is retrieved from the Crazyflie, @@ -570,7 +577,7 @@ def _init_parameters(self): for param in sorted(p_toc[group].keys()): name = group + "." + param - # Check the parameter type + # Check the parameter type elem = p_toc[group][param] type_cf_param = elem.ctype parameter_descriptor = ParameterDescriptor(type=cf_log_to_ros_param[type_cf_param]) @@ -680,9 +687,9 @@ def _parameters_callback(self, params): except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) - + return SetParametersResult(successful=False) - + def _emergency_callback(self, request, response, uri="all"): if uri == "all": for link_uri in self.uris: @@ -791,11 +798,75 @@ def _notify_setpoints_stop_callback(self, request, response, uri="all"): def _upload_trajectory_callback(self, request, response, uri="all"): self.get_logger().info("Notify trajectory not yet implemented") return response - + + def _upload_bezier_trajectory_callback(self, request, response, uri="all"): + id = request.trajectory_id + offset = request.piece_offset + length = len(request.pieces) + total_duration = 0 + + self.get_logger().info( + "upload_trajectory(id=%d, offset=%d, length=%d)" % (id, offset, length) + ) + + + # create the trajectories + # example from cflib + a = 0.9 + b = 0.5 + c = 0.5 + trajectory = [ + CompressedStart(0.0, 0.0, 0.0, 0.0), + CompressedSegment(2.0, [0.0, 1.0, 1.0], [0.0, a, 0.0], [], []), + CompressedSegment(2.0, [1.0, b, 0.0], [-a, -c, 0.0], [], []), + CompressedSegment(2.0, [-b, -1.0, -1.0], [c, a, 0.0], [], []), + CompressedSegment(2.0, [-1.0, 0.0, 0.0], [-a, 0.0, 0.0], [], []), + ] + + trajectory = [] + trajectory.append(CompressedStart(0.0, 0.0, 0.0, 0.0)) + for i in range(length): + piece = request.pieces[i] + duration = float(piece.duration.sec) + \ + float(piece.duration.nanosec)/1e9 + p = CompressedSegment(duration, piece.bezier_control_pts_x, piece.bezier_control_pts_y, piece.bezier_control_pts_z, piece.bezier_control_pts_yaw) + trajectory.append(p) + total_duration += duration + + if uri == "all": + upload_success_all = True + for link_uri in self.uris: + trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ)[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") + upload_success_all = False + else: + self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( + id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) + if upload_success_all is False: + response.success = False + return response + else: + trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ)[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") + response.success = False + return response + self.swarm._cfs[uri].cf.high_level_commander.define_trajectory( + id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) + + return response + def _start_trajectory_callback(self, request, response, uri="all"): self.get_logger().info("Start trajectory not yet implemented") return response - + def _poses_changed(self, msg): """ Topic update callback to the motion capture lib's @@ -813,7 +884,7 @@ def _poses_changed(self, msg): if name in self.uri_dict.keys(): uri = self.uri_dict[name] - #self.get_logger().info(f"{uri}: send extpos {x}, {y}, {z} to {name}") + # self.get_logger().info(f"{uri}: send extpos {x}, {y}, {z} to {name}") if isnan(quat.x): self.swarm._cfs[uri].cf.extpos.send_extpos( x, y, z) @@ -821,7 +892,6 @@ def _poses_changed(self, msg): self.swarm._cfs[uri].cf.extpos.send_extpose( x, y, z, quat.x, quat.y, quat.z, quat.w) - def _cmd_vel_legacy_changed(self, msg, uri=""): """ Topic update callback to control the attitude and thrust @@ -914,7 +984,7 @@ def _add_logging(self, request, response, uri="all"): lg_custom.add_variable(log_name) self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = self.create_publisher( LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10) - + self.swarm._cfs[uri].cf.log.add_config(lg_custom) lg_custom.data_received_cb.add_callback( @@ -944,7 +1014,7 @@ def _add_logging(self, request, response, uri="all"): response.success = True return response - + def main(args=None): diff --git a/crazyflie_examples/crazyflie_examples/bezier_test.py b/crazyflie_examples/crazyflie_examples/bezier_test.py new file mode 100644 index 000000000..f9744d119 --- /dev/null +++ b/crazyflie_examples/crazyflie_examples/bezier_test.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python + +import numpy as np +from pathlib import Path + +from crazyflie_py import * +from crazyflie_py.crazyflie_py.crazyswarm_py import Crazyswarm + + +class BezierTrajectoryPiece: + def __init__(self, bezier_curve): + control_points = np.array(bezier_curve["control_points"]) + + self.duration = bezier_curve["parameters"][0] + self.x = control_points[:, 0] + self.y = control_points[:, 1] + self.z = control_points[:, 2] + self.yaw = np.zeros_like(self.x) # assume zero yaw for now + + +def main(): + swarm = Crazyswarm() + timeHelper = swarm.timeHelper + allcfs = swarm.allcfs + + # TODO: construct the trajectory + bezier_curve = { + "control_points": [ + [0.0, -1.0, 1.0], + [0.0, -0.9999999999999999, 0.9999999999999999], + [-0.07534305719147254, -0.9943341183411414, 1.000090823889544], + [-0.19988495750934981, -0.9776819294183798, 1.0002323866263076], + [-0.347480864044028, -0.9447239205531746, 1.0003846034356692], + [-0.49199621358744006, -0.890133312602044, 1.0005073942908271], + [-0.6072947829139111, -0.8085840281358084, 1.0005606778422909], + ], + "parameters": [5.0], + } + # Construct the bezier trajectory, ideally this should be done within uploadBezierTrajectory + traj = BezierTrajectoryPiece(bezier_curve) + traj = [traj] + + TRIALS = 1 + TIMESCALE = 1.0 + for i in range(TRIALS): + for cf in allcfs.crazyflies: + cf.uploadBezierTrajectory(0, 0, trajectory=traj) + + allcfs.takeoff(targetHeight=1.0, duration=2.0) + timeHelper.sleep(2.5) + for cf in allcfs.crazyflies: + pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) + cf.goTo(pos, 0, 4.0) + timeHelper.sleep(4.5) + + allcfs.startTrajectory(0, timescale=TIMESCALE) + timeHelper.sleep(traj.duration * TIMESCALE + 2.0) + + timeHelper.sleep(4.5) + allcfs.land(targetHeight=0.0, duration=2.0) + timeHelper.sleep(3.0) + + +if __name__ == "__main__": + main() diff --git a/crazyflie_interfaces/CMakeLists.txt b/crazyflie_interfaces/CMakeLists.txt index f5b8b6cc7..11b15a01c 100644 --- a/crazyflie_interfaces/CMakeLists.txt +++ b/crazyflie_interfaces/CMakeLists.txt @@ -24,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/LogBlock.msg" "msg/Position.msg" "msg/TrajectoryPolynomialPiece.msg" + "msg/TrajectoryBezierPiece.msg" "msg/VelocityWorld.msg" "srv/GoTo.srv" "srv/Land.srv" @@ -34,6 +35,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/Takeoff.srv" "srv/UpdateParams.srv" "srv/UploadTrajectory.srv" + "srv/UploadBezierTrajectory.srv" "srv/RemoveLogging.srv" "srv/AddLogging.srv" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs diff --git a/crazyflie_interfaces/msg/TrajectoryBezierPiece.msg b/crazyflie_interfaces/msg/TrajectoryBezierPiece.msg new file mode 100644 index 000000000..72399e8e1 --- /dev/null +++ b/crazyflie_interfaces/msg/TrajectoryBezierPiece.msg @@ -0,0 +1,5 @@ +float32[] bezier_control_pts_x +float32[] bezier_control_pts_y +float32[] bezier_control_pts_z +float32[] bezier_control_pts_yaw +builtin_interfaces/Duration duration \ No newline at end of file diff --git a/crazyflie_interfaces/srv/UploadBezierTrajectory.srv b/crazyflie_interfaces/srv/UploadBezierTrajectory.srv new file mode 100644 index 000000000..7ce8b5695 --- /dev/null +++ b/crazyflie_interfaces/srv/UploadBezierTrajectory.srv @@ -0,0 +1,4 @@ +uint8 trajectory_id +uint32 piece_offset +TrajectoryBezierPiece[] pieces +--- diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 57c41adc8..af166f74f 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -34,11 +34,13 @@ Land, GoTo, UploadTrajectory, + UploadBezierTrajectory, StartTrajectory, NotifySetpointsStop, ) from crazyflie_interfaces.msg import ( TrajectoryPolynomialPiece, + TrajectoryBezierPiece, FullState, Position, VelocityWorld, @@ -142,6 +144,10 @@ def __init__(self, node, cfname, paramTypeDict, index=0, log_pose=False): UploadTrajectory, prefix + "/upload_trajectory" ) self.uploadTrajectoryService.wait_for_service() + self.uploadBezierTrajectoryService = node.create_client( + UploadBezierTrajectory, prefix + "/upload_bezier_trajectory" + ) + self.uploadBezierTrajectoryService.wait_for_service() self.startTrajectoryService = node.create_client( StartTrajectory, prefix + "/start_trajectory" ) @@ -455,6 +461,31 @@ def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory): req.piece_offset = pieceOffset req.pieces = pieces self.uploadTrajectoryService.call_async(req) + + def uploadBezierTrajectory(self, trajectoryId, pieceOffset, trajectory): + """_summary_ + + Args: + trajectoryId (_type_): _description_ + pieceOffset (_type_): _description_ + trajectory (_type_): _description_ + """ + pieces: list[TrajectoryBezierPiece] = [] + + for traj in trajectory: + piece = TrajectoryBezierPiece() + piece.duration = rclpy.duration.Duration(seconds=traj.duration).to_msg() + piece.bezier_control_pts_x = traj.x.tolist() + piece.bezier_control_pts_y = traj.y.tolist() + piece.bezier_control_pts_z = traj.z.tolist() + piece.bezier_control_pts_yaw = traj.yaw.tolist() + pieces.append(piece) + + req = UploadBezierTrajectory.Request() + req.trajectory_id = trajectoryId + req.piece_offset = pieceOffset + req.pieces = pieces + self.uploadBezierTrajectoryService.call_async(req) def startTrajectory( self, trajectoryId, timescale=1.0, reverse=False, relative=True, groupMask=0 From 89c5ecbdf9b1b0ee83ce584f9e8993cf015099cd Mon Sep 17 00:00:00 2001 From: qrthom Date: Fri, 5 Jul 2024 23:10:53 -0400 Subject: [PATCH 02/10] Remove wrong import --- crazyflie_examples/crazyflie_examples/bezier_test.py | 1 - 1 file changed, 1 deletion(-) diff --git a/crazyflie_examples/crazyflie_examples/bezier_test.py b/crazyflie_examples/crazyflie_examples/bezier_test.py index f9744d119..aef57ab86 100644 --- a/crazyflie_examples/crazyflie_examples/bezier_test.py +++ b/crazyflie_examples/crazyflie_examples/bezier_test.py @@ -4,7 +4,6 @@ from pathlib import Path from crazyflie_py import * -from crazyflie_py.crazyflie_py.crazyswarm_py import Crazyswarm class BezierTrajectoryPiece: From e55afd7f763138c274c6f9a1884c1ae511986b7c Mon Sep 17 00:00:00 2001 From: qrthom Date: Sat, 6 Jul 2024 00:55:09 -0400 Subject: [PATCH 03/10] WORKING bezier curve from examples --- crazyflie/scripts/crazyflie_server.py | 788 +++++++++---- crazyflie/scripts/old_crazyflie_server.py | 1032 +++++++++++++++++ .../crazyflie_examples/bezier_test.py | 3 +- 3 files changed, 1580 insertions(+), 243 deletions(-) create mode 100644 crazyflie/scripts/old_crazyflie_server.py diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index d769a7650..f758dbe3f 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -24,15 +24,21 @@ from cflib.crazyflie.mem import CompressedStart from cflib.crazyflie.log import LogConfig from cflib.crazyflie.high_level_commander import HighLevelCommander +from cflib.crazyflie.mem import Poly4D from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging -from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop, UploadBezierTrajectory +from crazyflie_interfaces.srv import ( + UploadTrajectory, + StartTrajectory, + NotifySetpointsStop, + UploadBezierTrajectory, +) from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType -from crazyflie_interfaces.msg import Hover -from crazyflie_interfaces.msg import LogDataGeneric +from crazyflie_interfaces.msg import Hover, LogDataGeneric, FullState from motion_capture_tracking_interfaces.msg import NamedPoseArray from std_srvs.srv import Empty +from std_msgs.msg import String from geometry_msgs.msg import Twist from geometry_msgs.msg import PoseStamped, TransformStamped from sensor_msgs.msg import LaserScan @@ -56,6 +62,7 @@ "double": ParameterType.PARAMETER_DOUBLE, } + class CrazyflieServer(Node): def __init__(self): super().__init__( @@ -68,24 +75,48 @@ def __init__(self): self._ros_parameters = self._param_to_dict(self._parameters) self.uris = [] - self.cf_dict = {} + # for logging, assign a all -> all mapping + 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']} - 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", + ], + "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.world_tf_name = "world" try: @@ -102,7 +133,9 @@ 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) @@ -114,7 +147,6 @@ 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: @@ -122,8 +154,7 @@ 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 ) @@ -136,15 +167,21 @@ 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 @@ -155,26 +192,36 @@ 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 @@ -188,17 +235,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: @@ -206,26 +262,23 @@ 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() - # Create services for the entire swarm and each individual crazyflie - self.create_service(Empty, "all/emergency", self._emergency_callback) - self.create_service(Takeoff, "all/takeoff", self._takeoff_callback) - self.create_service(Land, "all/land", self._land_callback) - self.create_service(GoTo, "all/go_to", self._go_to_callback) - self.create_service(StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) - for uri in self.cf_dict: + if uri == "all": + continue + name = self.cf_dict[uri] + self.create_service( - Empty, name + - "/emergency", partial(self._emergency_callback, uri=uri) + Empty, name + "/emergency", partial(self._emergency_callback, uri=uri) ) self.create_service( - Takeoff, name + - "/takeoff", partial(self._takeoff_callback, uri=uri) + Takeoff, name + "/takeoff", partial(self._takeoff_callback, uri=uri) ) self.create_service( Land, name + "/land", partial(self._land_callback, uri=uri) @@ -234,35 +287,69 @@ def __init__(self): GoTo, name + "/go_to", partial(self._go_to_callback, uri=uri) ) self.create_service( - StartTrajectory, name + "/start_trajectory", partial(self._start_trajectory_callback, uri=uri) + StartTrajectory, + name + "/start_trajectory", + partial(self._start_trajectory_callback, uri=uri), ) self.create_service( - UploadTrajectory, name + "/upload_trajectory", partial(self._upload_trajectory_callback, uri=uri) + UploadTrajectory, + name + "/upload_trajectory", + partial(self._upload_trajectory_callback, uri=uri), ) self.create_service( - UploadBezierTrajectory, name + "/upload_bezier_trajectory", partial(self._upload_bezier_trajectory_callback, uri=uri) + UploadBezierTrajectory, + name + "/upload_bezier_trajectory", + partial(self._upload_bezier_trajectory_callback, uri=uri), ) self.create_service( - NotifySetpointsStop, name + "/notify_setpoints_stop", partial(self._notify_setpoints_stop_callback, uri=uri) + NotifySetpointsStop, + name + "/notify_setpoints_stop", + partial(self._notify_setpoints_stop_callback, uri=uri), ) self.create_subscription( - Twist, name + - "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, uri=uri), 10 + Twist, + name + "/cmd_vel_legacy", + partial(self._cmd_vel_legacy_changed, uri=uri), + 10, ) self.create_subscription( - Hover, name + - "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 + Hover, + name + "/cmd_hover", + partial(self._cmd_hover_changed, uri=uri), + 10, + ) + + self.create_subscription( + FullState, + name + "/cmd_full_state", + partial(self._cmd_full_state_changed, uri=uri), + 10, ) - qos_profile = QoSProfile(reliability =QoSReliabilityPolicy.BEST_EFFORT, + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=1, - deadline = Duration(seconds=0, nanoseconds=1e9/100.0)) + deadline=Duration(seconds=0, nanoseconds=1e9 / 100.0), + ) + self.create_subscription( - NamedPoseArray, "/poses", - self._poses_changed, qos_profile + NamedPoseArray, "/poses", self._poses_changed, qos_profile ) - def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type): + # Create services for the entire swarm and each individual crazyflie + self.create_service(Takeoff, "all/takeoff", self._takeoff_callback) + self.create_service(Land, "all/land", self._land_callback) + 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) + + def _init_default_logblocks( + self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type + ): """ Prepare default logblocks as defined in crazyflies.yaml """ @@ -272,26 +359,28 @@ def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_ 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") @@ -302,8 +391,11 @@ def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_ 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" @@ -314,8 +406,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, {}) @@ -326,11 +418,12 @@ def _fully_connected(self, link_uri): Called when all parameters have been updated and the full log toc has been received of the Crazyflie """ - self.get_logger().info(f" {link_uri} is fully connected!") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] is fully connected!") self.swarm.fully_connected_crazyflie_cnt += 1 - if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict): + # 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() @@ -339,10 +432,10 @@ def _fully_connected(self, link_uri): return def _disconnected(self, link_uri): - self.get_logger().info(f" {link_uri} is disconnected!") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] is disconnected!") def _connection_failed(self, link_uri, msg): - self.get_logger().info(f"{link_uri} connection Failed") + self.get_logger().info(f"[{self.cf_dict[link_uri]}] connection Failed") self.swarm.close_links() def _init_logging(self): @@ -358,40 +451,64 @@ def _init_logging(self): # Start logging for predefined logging for default_log_name in self.default_log_type: prefix = default_log_name - if cf_handle.logging[prefix + "_logging_enabled"] and cf_handle.logging["enabled"]: + if ( + cf_handle.logging[prefix + "_logging_enabled"] + and cf_handle.logging["enabled"] + ): callback_fnc = self.default_log_fnc[prefix] self._init_default_logging(prefix, link_uri, callback_fnc) # Start logging for costum logging blocks cf_handle.l_toc = cf.log.toc.toc - if len(cf_handle.logging["custom_log_groups"]) != 0 and cf_handle.logging["enabled"]: - - for log_group_name, log_group_dict in cf_handle.logging["custom_log_groups"].items(): - self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = self.create_publisher( - LogDataGeneric, self.cf_dict[link_uri] + "/" + log_group_name, 10) - lg_custom = log_group_dict['log_config'] + if ( + len(cf_handle.logging["custom_log_groups"]) != 0 + and cf_handle.logging["enabled"] + ): + + for log_group_name, log_group_dict in cf_handle.logging[ + "custom_log_groups" + ].items(): + self.swarm._cfs[link_uri].logging["custom_log_publisher"][ + log_group_name + ] = self.create_publisher( + LogDataGeneric, + self.cf_dict[link_uri] + "/" + log_group_name, + 10, + ) + lg_custom = log_group_dict["log_config"] try: cf.log.add_config(lg_custom) lg_custom.data_received_cb.add_callback( - partial(self._log_custom_data_callback, uri=link_uri)) - lg_custom.error_cb.add_callback( - self._log_error_callback) + partial(self._log_custom_data_callback, uri=link_uri) + ) + lg_custom.error_cb.add_callback(self._log_error_callback) lg_custom.start() except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' - '{} not found in TOC'.format(str(e))) + self.get_logger().info( + f"[{self.cf_dict[link_uri]}] Could not start log configuration," + "{} not found in TOC".format(str(e)) + ) except AttributeError: self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') + f"[{self.cf_dict[link_uri]}] Could not add log config, bad configuration." + ) - self.get_logger().info(f"{link_uri} setup custom logging") + self.get_logger().info( + f"[{self.cf_dict[link_uri]}] setup custom logging" + ) self.create_service( - RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri)) + RemoveLogging, + self.cf_dict[link_uri] + "/remove_logging", + partial(self._remove_logging, uri=link_uri), + ) self.create_service( - AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri)) + AddLogging, + self.cf_dict[link_uri] + "/add_logging", + partial(self._add_logging, uri=link_uri), + ) - self.get_logger().info("All Crazyflies loggging are initialized") + self.get_logger().info("All Crazyflies logging are initialized.") def _init_default_logging(self, prefix, link_uri, callback_fnc): """ @@ -402,33 +519,37 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc): lg = cf_handle.logging[prefix + "_log_config"] try: cf.log.add_config(lg) - lg.data_received_cb.add_callback( - partial(callback_fnc, uri=link_uri)) + lg.data_received_cb.add_callback(partial(callback_fnc, uri=link_uri)) lg.error_cb.add_callback(self._log_error_callback) lg.start() frequency = cf_handle.logging[prefix + "_logging_freq"] self.declare_parameter( - self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency) + self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency + ) self.get_logger().info( - f"{link_uri} setup logging for {prefix} at freq {frequency}") + f"[{self.cf_dict[link_uri]}] setup logging for {prefix} at freq {frequency}" + ) except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' - '{} not found in TOC'.format(str(e))) + self.get_logger().error( + f"[{self.cf_dict[link_uri]}] Could not start log configuration," + "{} not found in TOC".format(str(e)) + ) except AttributeError: - self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') + self.get_logger().error( + f"[{self.cf_dict[link_uri]}] Could not add log config, bad configuration." + ) 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] max_range = 3.49 - front_range = float(data.get('range.front'))/1000.0 - left_range = float(data.get('range.left'))/1000.0 - back_range = float(data.get('range.back'))/1000.0 - right_range = float(data.get('range.right'))/1000.0 + front_range = float(data.get("range.front")) / 1000.0 + left_range = float(data.get("range.left")) / 1000.0 + back_range = float(data.get("range.back")) / 1000.0 + right_range = float(data.get("range.right")) / 1000.0 if front_range > max_range: front_range = float("inf") if left_range > max_range: @@ -436,7 +557,7 @@ def _log_scan_data_callback(self, timestamp, data, logconf, uri): if right_range > max_range: right_range = float("inf") if back_range > max_range: - back_range = float("inf") + back_range = float("inf") self.ranges = [back_range, right_range, front_range, left_range] msg = LaserScan() @@ -445,25 +566,25 @@ def _log_scan_data_callback(self, timestamp, data, logconf, uri): msg.range_min = 0.01 msg.range_max = 3.49 msg.ranges = self.ranges - msg.angle_min = -0.5 * 2* pi - msg.angle_max = 0.25 * 2 * pi - msg.angle_increment = 1.0 * pi/2 + msg.angle_min = -0.5 * 2 * pi + msg.angle_max = 0.25 * 2 * pi + msg.angle_increment = 1.0 * pi / 2 self.swarm._cfs[uri].logging["scan_publisher"].publish(msg) 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 """ cf_name = self.cf_dict[uri] - x = data.get('stateEstimate.x') - y = data.get('stateEstimate.y') - z = data.get('stateEstimate.z') - roll = radians(data.get('stabilizer.roll')) - pitch = radians(-1.0 * data.get('stabilizer.pitch')) - yaw = radians(data.get('stabilizer.yaw')) + x = data.get("stateEstimate.x") + y = data.get("stateEstimate.y") + z = data.get("stateEstimate.z") + roll = radians(data.get("stabilizer.roll")) + pitch = radians(-1.0 * data.get("stabilizer.pitch")) + yaw = radians(data.get("stabilizer.yaw")) q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) msg = PoseStamped() @@ -493,23 +614,23 @@ 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] - x = data.get('stateEstimate.x') - y = data.get('stateEstimate.y') - z = data.get('stateEstimate.z') - yaw = radians(data.get('stabilizer.yaw')) - roll = radians(data.get('stabilizer.roll')) - pitch = radians(data.get('stabilizer.pitch')) - vx = data.get('kalman.statePX') - vy = data.get('kalman.statePY') - vz = data.get('kalman.statePY') - yawrate = data.get('gyro.z') - rollrate = data.get('gyro.x') - pitchrate = data.get('gyro.y') + x = data.get("stateEstimate.x") + y = data.get("stateEstimate.y") + z = data.get("stateEstimate.z") + yaw = radians(data.get("stabilizer.yaw")) + roll = radians(data.get("stabilizer.roll")) + pitch = radians(data.get("stabilizer.pitch")) + vx = data.get("kalman.statePX") + vy = data.get("kalman.statePY") + vz = data.get("kalman.statePZ") + yawrate = data.get("gyro.z") + rollrate = data.get("gyro.x") + pitchrate = data.get("gyro.y") q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) msg = Odometry() @@ -534,7 +655,7 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri): t_base = TransformStamped() t_base.header.stamp = self.get_clock().now().to_msg() - t_base.header.frame_id = 'odom' + t_base.header.frame_id = "odom" t_base.child_frame_id = cf_name t_base.transform.translation.x = x t_base.transform.translation.y = y @@ -547,7 +668,7 @@ def _log_odom_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() @@ -556,15 +677,14 @@ def _log_custom_data_callback(self, timestamp, data, logconf, uri): for log_name in data: msg.values.append(data.get(log_name)) - self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish( - msg) + self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish(msg) def _log_error_callback(self, logconf, msg): - print('Error when logging %s: %s' % (logconf.name, msg)) + print("Error when logging %s: %s" % (logconf.name, 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 @@ -580,22 +700,30 @@ def _init_parameters(self): # Check the parameter type elem = p_toc[group][param] type_cf_param = elem.ctype - parameter_descriptor = ParameterDescriptor(type=cf_log_to_ros_param[type_cf_param]) + parameter_descriptor = ParameterDescriptor( + type=cf_log_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 try: - set_param_value = self._ros_parameters["all"]["firmware_params"][group][param] + set_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]]["firmware_params"][group][param] + set_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]]["firmware_params"][group][param] + set_param_value = self._ros_parameters["robots"][ + self.cf_dict[link_uri] + ]["firmware_params"][group][param] except KeyError: pass @@ -606,11 +734,10 @@ def _init_parameters(self): # crazyflie with get_value due to threading. cf.param.set_value(name, set_param_value) self.get_logger().info( - f" {link_uri}: {name} is set to {set_param_value}" + f"[{self.cf_dict[link_uri]}] {name} is set to {set_param_value}" ) self.declare_parameter( - self.cf_dict[link_uri] + - ".params." + group + "." + param, + self.cf_dict[link_uri] + ".params." + group + "." + param, value=set_param_value, descriptor=parameter_descriptor, ) @@ -619,21 +746,32 @@ def _init_parameters(self): # If value is not found in initial parameter set # get crazyflie paramter value and declare that value in ROS 2 parameter - if cf_log_to_ros_param[type_cf_param] is ParameterType.PARAMETER_INTEGER: + 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: + elif ( + cf_log_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, + 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: + 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: + 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( @@ -642,10 +780,10 @@ def _init_parameters(self): descriptor=parameter_descriptor, ) - # Now all parameters are set + # Now all parameters are set set_param_all = True - self.get_logger().info("All Crazyflies parameters are initialized") + self.get_logger().info("All Crazyflies parameters are initialized.") def _parameters_callback(self, params): """ @@ -655,38 +793,38 @@ def _parameters_callback(self, params): for param in params: param_split = param.name.split(".") - if param_split[0] in self.cf_dict.values(): - cf_name = param_split[0] + if param_split[0] == "all": if param_split[1] == "params": name_param = param_split[2] + "." + param_split[3] try: - self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( - name_param, param.value - ) + for link_uri in self.uris: + cf = self.swarm._cfs[link_uri].cf.param.set_value( + name_param, param.value + ) self.get_logger().info( - f" {self.uri_dict[cf_name]}: {name_param} is set to {param.value}" + f"[{self.cf_dict[link_uri]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) - if param_split[1] == "logs": - return SetParametersResult(successful=True) - elif param_split[0] == "all": + elif param_split[0] in self.cf_dict.values(): + cf_name = param_split[0] if param_split[1] == "params": name_param = param_split[2] + "." + param_split[3] try: - for link_uri in self.uris: - cf = self.swarm._cfs[link_uri].cf.param.set_value( + self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( name_param, param.value ) self.get_logger().info( - f" {link_uri}: {name_param} is set to {param.value}" + f"[{self.uri_dict[cf_name]}] {name_param} is set to {param.value}" ) return SetParametersResult(successful=True) except Exception as e: self.get_logger().info(str(e)) return SetParametersResult(successful=False) + if param_split[1] == "logs": + return SetParametersResult(successful=True) return SetParametersResult(successful=False) @@ -701,16 +839,17 @@ 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 """ - duration = float(request.duration.sec) + \ - float(request.duration.nanosec / 1e9) + print("call1 ", uri) + + duration = float(request.duration.sec) + float(request.duration.nanosec / 1e9) self.get_logger().info( - f"takeoff(height={request.height} m," + f"[{self.cf_dict[uri]}] takeoff(height={request.height} m," + f"duration={duration} s," - + f"group_mask={request.group_mask}) {uri}" + + f"group_mask={request.group_mask})" ) if uri == "all": for link_uri in self.uris: @@ -726,13 +865,12 @@ 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) + \ - float(request.duration.nanosec / 1e9) + duration = float(request.duration.sec) + float(request.duration.nanosec / 1e9) self.get_logger().info( - f"land(height={request.height} m," + f"[{self.cf_dict[uri]}] land(height={request.height} m," + f"duration={duration} s," + f"group_mask={request.group_mask})" ) @@ -750,15 +888,15 @@ 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) + \ - float(request.duration.nanosec / 1e9) + duration = float(request.duration.sec) + float(request.duration.nanosec / 1e9) self.get_logger().info( - "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" + "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" % ( + self.cf_dict[uri], request.goal.x, request.goal.y, request.goal.z, @@ -792,11 +930,76 @@ def _go_to_callback(self, request, response, uri="all"): return response def _notify_setpoints_stop_callback(self, request, response, uri="all"): - self.get_logger().info("Notify setpoint stop not yet implemented") + + self.get_logger().info(f"[{self.cf_dict[uri]}] Received notify setpoint stop") + + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.commander.send_notify_setpoint_stop() + else: + self.swarm._cfs[uri].cf.commander.send_notify_setpoint_stop() + return response def _upload_trajectory_callback(self, request, response, uri="all"): - self.get_logger().info("Notify trajectory not yet implemented") + + id = request.trajectory_id + offset = request.piece_offset + lenght = len(request.pieces) + total_duration = 0 + self.get_logger().info( + "[%s] upload_trajectory(id=%d,offset=%d, lenght=%d)" + % ( + self.cf_dict[uri], + id, + offset, + lenght, + ) + ) + + trajectory = [] + for i in range(lenght): + piece = request.pieces[i] + px = Poly4D.Poly(piece.poly_x) + py = Poly4D.Poly(piece.poly_y) + pz = Poly4D.Poly(piece.poly_z) + pyaw = Poly4D.Poly(piece.poly_yaw) + duration = float(piece.duration.sec) + float(piece.duration.nanosec) / 1e9 + trajectory.append(Poly4D(duration, px, py, pz, pyaw)) + total_duration = total_duration + duration + + if uri == "all": + upload_success_all = True + for link_uri in self.uris: + trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ + )[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed") + upload_success_all = False + else: + self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( + id, offset, len(trajectory) + ) + if upload_success_all is False: + response.success = False + return response + else: + trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ + )[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload failed") + response.success = False + return response + self.swarm._cfs[uri].cf.high_level_commander.define_trajectory( + id, offset, len(trajectory) + ) + return response def _upload_bezier_trajectory_callback(self, request, response, uri="all"): @@ -808,7 +1011,6 @@ def _upload_bezier_trajectory_callback(self, request, response, uri="all"): self.get_logger().info( "upload_trajectory(id=%d, offset=%d, length=%d)" % (id, offset, length) ) - # create the trajectories # example from cflib @@ -822,56 +1024,98 @@ def _upload_bezier_trajectory_callback(self, request, response, uri="all"): CompressedSegment(2.0, [-b, -1.0, -1.0], [c, a, 0.0], [], []), CompressedSegment(2.0, [-1.0, 0.0, 0.0], [-a, 0.0, 0.0], [], []), ] - - trajectory = [] - trajectory.append(CompressedStart(0.0, 0.0, 0.0, 0.0)) - for i in range(length): - piece = request.pieces[i] - duration = float(piece.duration.sec) + \ - float(piece.duration.nanosec)/1e9 - p = CompressedSegment(duration, piece.bezier_control_pts_x, piece.bezier_control_pts_y, piece.bezier_control_pts_z, piece.bezier_control_pts_yaw) - trajectory.append(p) - total_duration += duration - + + # trajectory = [] + # trajectory.append(CompressedStart(0.0, 0.0, 0.0, 0.0)) + # for i in range(length): + # piece = request.pieces[i] + # duration = float(piece.duration.sec) + float(piece.duration.nanosec) / 1e9 + # p = CompressedSegment( + # duration, + # piece.bezier_control_pts_x, + # piece.bezier_control_pts_y, + # piece.bezier_control_pts_z, + # piece.bezier_control_pts_yaw, + # ) + # trajectory.append(p) + # total_duration += duration + if uri == "all": upload_success_all = True for link_uri in self.uris: trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( - MemoryElement.TYPE_TRAJ)[id] + MemoryElement.TYPE_TRAJ + )[id] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: - self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") + self.get_logger().info( + f"[{self.cf_dict[uri]}] Upload bezier trajectory failed" + ) upload_success_all = False else: + self.get_logger().info( + f"[{self.cf_dict[uri]}] Upload bezier trajectory success" + ) self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( - id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) + id, + offset, + len(trajectory), + type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED, + ) if upload_success_all is False: response.success = False return response else: trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( - MemoryElement.TYPE_TRAJ)[id] + MemoryElement.TYPE_TRAJ + )[id] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: - self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") + self.get_logger().info( + f"[{self.cf_dict[uri]}] Upload bezier trajectory failed" + ) response.success = False return response self.swarm._cfs[uri].cf.high_level_commander.define_trajectory( - id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) + id, + offset, + len(trajectory), + type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED, + ) return response def _start_trajectory_callback(self, request, response, uri="all"): - self.get_logger().info("Start trajectory not yet implemented") + + id = request.trajectory_id + ts = request.timescale + rel = request.relative + rev = request.reversed + gm = request.group_mask + + self.get_logger().info( + "[%s] start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" + % (self.cf_dict[uri], id, ts, rel, rev, gm) + ) + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.high_level_commander.start_trajectory( + id, ts, rel, rev, gm + ) + else: + self.swarm._cfs[uri].cf.high_level_commander.start_trajectory( + id, ts, rel, rev, gm + ) + return response 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 @@ -886,11 +1130,11 @@ def _poses_changed(self, msg): uri = self.uri_dict[name] # self.get_logger().info(f"{uri}: send extpos {x}, {y}, {z} to {name}") if isnan(quat.x): - self.swarm._cfs[uri].cf.extpos.send_extpos( - x, y, z) + self.swarm._cfs[uri].cf.extpos.send_extpos(x, y, z) else: self.swarm._cfs[uri].cf.extpos.send_extpose( - x, y, z, quat.x, quat.y, quat.z, quat.w) + x, y, z, quat.x, quat.y, quat.z, quat.w + ) def _cmd_vel_legacy_changed(self, msg, uri=""): """ @@ -901,8 +1145,7 @@ def _cmd_vel_legacy_changed(self, msg, uri=""): pitch = -msg.linear.x yawrate = msg.angular.z thrust = int(min(max(msg.linear.z, 0, 0), 60000)) - self.swarm._cfs[uri].cf.commander.send_setpoint( - roll, pitch, yawrate, thrust) + self.swarm._cfs[uri].cf.commander.send_setpoint(roll, pitch, yawrate, thrust) def _cmd_hover_changed(self, msg, uri=""): """ @@ -912,9 +1155,28 @@ def _cmd_hover_changed(self, msg, uri=""): vx = msg.vx vy = msg.vy z = msg.z_distance - yawrate = -1.0*degrees(msg.yaw_rate) + yawrate = -1.0 * degrees(msg.yaw_rate) self.swarm._cfs[uri].cf.commander.send_hover_setpoint(vx, vy, yawrate, z) - self.get_logger().info(f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") + + def _cmd_full_state_changed(self, msg, uri=""): + """ + Topic update callback to full state cmd topic + """ + pos = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z] + vel = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z] + acc = [msg.acc.x, msg.acc.y, msg.acc.z] + q = [ + msg.pose.orientation.x, + msg.pose.orientation.y, + msg.pose.orientation.z, + msg.pose.orientation.w, + ] + roll_rate = msg.twist.angular.x + 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"): """ @@ -924,27 +1186,39 @@ def _remove_logging(self, request, response, uri="all"): if topic_name in self.default_log_type.keys(): try: self.undeclare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") + self.cf_dict[uri] + ".logs." + topic_name + ".frequency." + ) self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop() self.destroy_publisher( - self.swarm._cfs[uri].logging[topic_name + "_publisher"]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") + self.swarm._cfs[uri].logging[topic_name + "_publisher"] + ) + self.get_logger().info( + f"[{self.cf_dict[uri]}] Remove {topic_name} logging" + ) except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found " + ) response.success = False return response else: try: - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"].stop( - ) - for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]: + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ + "log_config" + ].stop() + for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][ + topic_name + ]["vars"]: self.destroy_publisher( - self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") + self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] + ) + self.get_logger().info( + f"[{self.cf_dict[uri]}] Remove {topic_name} logging" + ) except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found " + ) response.success = False return response @@ -961,54 +1235,84 @@ def _add_logging(self, request, response, uri="all"): if topic_name in self.default_log_type.keys(): try: self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) - self.swarm._cfs[uri].logging[topic_name + "_publisher"] = self.create_publisher( - self.default_log_type[topic_name], self.cf_dict[uri] + "/" + topic_name, 10) - self.swarm._cfs[uri].logging[topic_name + "_log_config"].period_in_ms = 1000 / frequency + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency + ) + self.swarm._cfs[uri].logging[topic_name + "_publisher"] = ( + self.create_publisher( + self.default_log_type[topic_name], + self.cf_dict[uri] + "/" + topic_name, + 10, + ) + ) + self.swarm._cfs[uri].logging[ + topic_name + "_log_config" + ].period_in_ms = (1000 / frequency) self.swarm._cfs[uri].logging[topic_name + "_log_config"].start() - self.get_logger().info(f"{uri}: Add {topic_name} logging") + self.get_logger().info( + f"[{self.cf_dict[uri]}] Add {topic_name} logging" + ) except rclpy.exceptions.ParameterAlreadyDeclaredException: self.get_logger().info( - f"{uri}: The content the logging of {topic_name} has already started ") + f"[{self.cf_dict[uri]}] The content the logging of {topic_name} has already started " + ) response.success = False return response else: try: self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency + ) self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".vars.", variables) - lg_custom = LogConfig( - name=topic_name, period_in_ms=1000 / frequency) + self.cf_dict[uri] + ".logs." + topic_name + ".vars.", variables + ) + lg_custom = LogConfig(name=topic_name, period_in_ms=1000 / frequency) for log_name in variables: lg_custom.add_variable(log_name) - self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = self.create_publisher( - LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10) + self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = ( + self.create_publisher( + LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10 + ) + ) self.swarm._cfs[uri].cf.log.add_config(lg_custom) lg_custom.data_received_cb.add_callback( - partial(self._log_custom_data_callback, uri=uri)) + partial(self._log_custom_data_callback, uri=uri) + ) lg_custom.error_cb.add_callback(self._log_error_callback) lg_custom.start() self.swarm._cfs[uri].logging["custom_log_groups"][topic_name] = {} - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"] = lg_custom - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ + "log_config" + ] = lg_custom + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ + "vars" + ] = variables + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ + "frequency" + ] = frequency - self.get_logger().info(f"{uri}: Add {topic_name} logging") - except KeyError as e: self.get_logger().info( - f"{uri}: Failed to add {topic_name} logging") - self.get_logger().info(str(e) + "is not in TOC") - self.undeclare_parameter(self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") - self.undeclare_parameter(self.cf_dict[uri] + ".logs." + topic_name + ".vars.") + f"[{self.cf_dict[uri]}] Add {topic_name} logging" + ) + except KeyError as e: + self.get_logger().error( + f"[{self.cf_dict[uri]}] Failed to add {topic_name} logging" + ) + self.get_logger().error(str(e) + "is not in TOC") + self.undeclare_parameter( + self.cf_dict[uri] + ".logs." + topic_name + ".frequency." + ) + self.undeclare_parameter( + self.cf_dict[uri] + ".logs." + topic_name + ".vars." + ) response.success = False return response except rclpy.exceptions.ParameterAlreadyDeclaredException: - self.get_logger().info( - f"{uri}: The content or part of the logging of {topic_name} has already started ") + self.get_logger().error( + f"[{self.cf_dict[uri]}] The content or part of the logging of {topic_name} has already started " + ) response.success = False return response diff --git a/crazyflie/scripts/old_crazyflie_server.py b/crazyflie/scripts/old_crazyflie_server.py new file mode 100644 index 000000000..d769a7650 --- /dev/null +++ b/crazyflie/scripts/old_crazyflie_server.py @@ -0,0 +1,1032 @@ +#!/usr/bin/env python3 + +""" +A crazyflie server for communicating with several crazyflies + based on the official crazyflie python library from + Bitcraze AB + + + 2022 - K. N. McGuire (Bitcraze AB) +""" + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from rclpy.duration import Duration + +import time + +import cflib.crtp +from cflib.crazyflie.swarm import CachedCfFactory +from cflib.crazyflie.swarm import Swarm +from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.mem import CompressedSegment +from cflib.crazyflie.mem import CompressedStart +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.high_level_commander import HighLevelCommander + +from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging +from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop, UploadBezierTrajectory +from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType +from crazyflie_interfaces.msg import Hover +from crazyflie_interfaces.msg import LogDataGeneric +from motion_capture_tracking_interfaces.msg import NamedPoseArray + +from std_srvs.srv import Empty +from geometry_msgs.msg import Twist +from geometry_msgs.msg import PoseStamped, TransformStamped +from sensor_msgs.msg import LaserScan +from nav_msgs.msg import Odometry + +import tf_transformations +from tf2_ros import TransformBroadcaster + +from functools import partial +from math import degrees, radians, pi, isnan + +cf_log_to_ros_param = { + "uint8_t": ParameterType.PARAMETER_INTEGER, + "uint16_t": ParameterType.PARAMETER_INTEGER, + "uint32_t": ParameterType.PARAMETER_INTEGER, + "int8_t": ParameterType.PARAMETER_INTEGER, + "int16_t": ParameterType.PARAMETER_INTEGER, + "int32_t": ParameterType.PARAMETER_INTEGER, + "FP16": ParameterType.PARAMETER_DOUBLE, + "float": ParameterType.PARAMETER_DOUBLE, + "double": ParameterType.PARAMETER_DOUBLE, +} + +class CrazyflieServer(Node): + def __init__(self): + super().__init__( + "crazyflie_server", + allow_undeclared_parameters=True, + automatically_declare_parameters_from_overrides=True, + ) + + # Turn ROS parameters into a dictionary + self._ros_parameters = self._param_to_dict(self._parameters) + + self.uris = [] + self.cf_dict = {} + 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']} + 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: + self.world_tf_name = self._ros_parameters["world_tf_name"] + except KeyError: + pass + robot_data = self._ros_parameters["robots"] + + # Init a transform broadcaster + self.tfbr = TransformBroadcaster(self) + + # Create easy lookup tables for uri, name and types + for crazyflie in robot_data: + 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") + if connection == "crazyflie": + uri = robot_data[crazyflie]["uri"] + self.uris.append(uri) + self.cf_dict[uri] = crazyflie + self.uri_dict[crazyflie] = uri + self.type_dict[uri] = type_cf + + # Setup Swarm class cflib with connection callbacks and open the links + 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.connection_failed.add_callback( + self._connection_failed + ) + + self.swarm._cfs[link_uri].logging = {} + + cf_name = self.cf_dict[link_uri] + cf_type = self.type_dict[link_uri] + + # check if logging is enabled at startup + logging_enabled = False + try: + 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"] + except KeyError: + pass + try: + logging_enabled = self._ros_parameters['robots'][cf_name]["firmware_logging"]["enabled"] + except KeyError: + pass + + self.swarm._cfs[link_uri].logging["enabled"] = logging_enabled + + # check if predefine log blocks can be logged and setup crazyflie logblocks and ROS 2 publishers + for default_log_name in self.default_log_type: + 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) + + # 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_logging_enabled = True + except KeyError: + pass + try: + custom_log_topics.update( + 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"]) + custom_logging_enabled = True + except KeyError: + pass + + self.swarm._cfs[link_uri].logging["custom_log_topics"] = {} + self.swarm._cfs[link_uri].logging["custom_log_groups"] = {} + self.swarm._cfs[link_uri].logging["custom_log_publisher"] = {} + + # Setup log blocks for each custom log and ROS 2 publisher topics + if custom_logging_enabled: + 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) + 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"] + + # 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") + exit() + + # Create services for the entire swarm and each individual crazyflie + self.create_service(Empty, "all/emergency", self._emergency_callback) + self.create_service(Takeoff, "all/takeoff", self._takeoff_callback) + self.create_service(Land, "all/land", self._land_callback) + self.create_service(GoTo, "all/go_to", self._go_to_callback) + self.create_service(StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) + + for uri in self.cf_dict: + name = self.cf_dict[uri] + self.create_service( + Empty, name + + "/emergency", partial(self._emergency_callback, uri=uri) + ) + self.create_service( + Takeoff, name + + "/takeoff", partial(self._takeoff_callback, uri=uri) + ) + self.create_service( + Land, name + "/land", partial(self._land_callback, uri=uri) + ) + self.create_service( + GoTo, name + "/go_to", partial(self._go_to_callback, uri=uri) + ) + self.create_service( + StartTrajectory, name + "/start_trajectory", partial(self._start_trajectory_callback, uri=uri) + ) + self.create_service( + UploadTrajectory, name + "/upload_trajectory", partial(self._upload_trajectory_callback, uri=uri) + ) + self.create_service( + UploadBezierTrajectory, name + "/upload_bezier_trajectory", partial(self._upload_bezier_trajectory_callback, uri=uri) + ) + self.create_service( + NotifySetpointsStop, name + "/notify_setpoints_stop", partial(self._notify_setpoints_stop_callback, uri=uri) + ) + self.create_subscription( + Twist, name + + "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, uri=uri), 10 + ) + self.create_subscription( + Hover, name + + "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 + ) + qos_profile = QoSProfile(reliability =QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + deadline = Duration(seconds=0, nanoseconds=1e9/100.0)) + self.create_subscription( + NamedPoseArray, "/poses", + self._poses_changed, qos_profile + ) + + def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type): + """ + Prepare default logblocks as defined in crazyflies.yaml + """ + cf_name = self.cf_dict[link_uri] + cf_type = self.type_dict[link_uri] + + logging_enabled = False + logging_freq = 10 + try: + 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_enabled = True + except KeyError: + pass + try: + 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) + for logvar in list_logvar: + if prefix == "odom": + lg.add_variable(logvar, "FP16") + else: + lg.add_variable(logvar) + + self.swarm._cfs[link_uri].logging[prefix + "_logging_enabled"] = logging_enabled + 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) + else: + self.swarm._cfs[link_uri].logging[prefix + "_publisher"] = "empty" + + def _param_to_dict(self, param_ros): + """ + Turn ROS 2 parameters from the node into a dict + """ + tree = {} + for item in param_ros: + t = tree + for part in item.split('.'): + if part == item.split('.')[-1]: + t = t.setdefault(part, param_ros[item].value) + else: + t = t.setdefault(part, {}) + return tree + + def _fully_connected(self, link_uri): + """ + Called when all parameters have been updated + and the full log toc has been received of the Crazyflie + """ + self.get_logger().info(f" {link_uri} is fully connected!") + + self.swarm.fully_connected_crazyflie_cnt += 1 + + if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict): + self.get_logger().info("All Crazyflies are fully connected!") + self._init_parameters() + self._init_logging() + self.add_on_set_parameters_callback(self._parameters_callback) + else: + return + + def _disconnected(self, link_uri): + self.get_logger().info(f" {link_uri} is disconnected!") + + def _connection_failed(self, link_uri, msg): + self.get_logger().info(f"{link_uri} connection Failed") + self.swarm.close_links() + + def _init_logging(self): + """ + Sets up all the log blocks for the crazyflie and + all the ROS 2 publisher and parameters for logging + at startup + """ + for link_uri in self.uris: + cf_handle = self.swarm._cfs[link_uri] + cf = cf_handle.cf + + # Start logging for predefined logging + for default_log_name in self.default_log_type: + prefix = default_log_name + if cf_handle.logging[prefix + "_logging_enabled"] and cf_handle.logging["enabled"]: + callback_fnc = self.default_log_fnc[prefix] + self._init_default_logging(prefix, link_uri, callback_fnc) + + # Start logging for costum logging blocks + cf_handle.l_toc = cf.log.toc.toc + if len(cf_handle.logging["custom_log_groups"]) != 0 and cf_handle.logging["enabled"]: + + for log_group_name, log_group_dict in cf_handle.logging["custom_log_groups"].items(): + self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = self.create_publisher( + LogDataGeneric, self.cf_dict[link_uri] + "/" + log_group_name, 10) + lg_custom = log_group_dict['log_config'] + try: + cf.log.add_config(lg_custom) + lg_custom.data_received_cb.add_callback( + partial(self._log_custom_data_callback, uri=link_uri)) + lg_custom.error_cb.add_callback( + self._log_error_callback) + lg_custom.start() + except KeyError as e: + self.get_logger().info(f'{link_uri}: Could not start log configuration,' + '{} not found in TOC'.format(str(e))) + except AttributeError: + self.get_logger().info( + f'{link_uri}: Could not add log config, bad configuration.') + + self.get_logger().info(f"{link_uri} setup custom logging") + + self.create_service( + RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri)) + self.create_service( + AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri)) + + self.get_logger().info("All Crazyflies loggging are initialized") + + def _init_default_logging(self, prefix, link_uri, callback_fnc): + """ + Sets up all the default log blocks and ROS 2 publishers for the crazyflie + """ + cf_handle = self.swarm._cfs[link_uri] + cf = cf_handle.cf + lg = cf_handle.logging[prefix + "_log_config"] + try: + cf.log.add_config(lg) + lg.data_received_cb.add_callback( + partial(callback_fnc, uri=link_uri)) + lg.error_cb.add_callback(self._log_error_callback) + lg.start() + frequency = cf_handle.logging[prefix + "_logging_freq"] + self.declare_parameter( + self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency) + self.get_logger().info( + f"{link_uri} setup logging for {prefix} at freq {frequency}") + except KeyError as e: + self.get_logger().info(f'{link_uri}: Could not start log configuration,' + '{} not found in TOC'.format(str(e))) + except AttributeError: + self.get_logger().info( + f'{link_uri}: Could not add log config, bad configuration.') + + def _log_scan_data_callback(self, timestamp, data, logconf, uri): + """ + Once multiranger range is retrieved from the Crazyflie, + send out the ROS 2 topic for Scan + """ + cf_name = self.cf_dict[uri] + max_range = 3.49 + front_range = float(data.get('range.front'))/1000.0 + left_range = float(data.get('range.left'))/1000.0 + back_range = float(data.get('range.back'))/1000.0 + right_range = float(data.get('range.right'))/1000.0 + if front_range > max_range: + front_range = float("inf") + if left_range > max_range: + left_range = float("inf") + if right_range > max_range: + right_range = float("inf") + if back_range > max_range: + back_range = float("inf") + self.ranges = [back_range, right_range, front_range, left_range] + + msg = LaserScan() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = cf_name + msg.range_min = 0.01 + msg.range_max = 3.49 + msg.ranges = self.ranges + msg.angle_min = -0.5 * 2* pi + msg.angle_max = 0.25 * 2 * pi + msg.angle_increment = 1.0 * pi/2 + self.swarm._cfs[uri].logging["scan_publisher"].publish(msg) + + def _log_pose_data_callback(self, timestamp, data, logconf, uri): + """ + Once pose data is retrieved from the Crazyflie, + send out the ROS 2 topic for Pose + """ + + cf_name = self.cf_dict[uri] + + x = data.get('stateEstimate.x') + y = data.get('stateEstimate.y') + z = data.get('stateEstimate.z') + roll = radians(data.get('stabilizer.roll')) + pitch = radians(-1.0 * data.get('stabilizer.pitch')) + yaw = radians(data.get('stabilizer.yaw')) + q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) + + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.world_tf_name + msg.pose.position.x = x + msg.pose.position.y = y + msg.pose.position.z = z + msg.pose.orientation.x = q[0] + msg.pose.orientation.y = q[1] + msg.pose.orientation.z = q[2] + msg.pose.orientation.w = q[3] + self.swarm._cfs[uri].logging["pose_publisher"].publish(msg) + + t_base = TransformStamped() + t_base.header.stamp = self.get_clock().now().to_msg() + t_base.header.frame_id = self.world_tf_name + t_base.child_frame_id = cf_name + t_base.transform.translation.x = x + t_base.transform.translation.y = y + t_base.transform.translation.z = z + t_base.transform.rotation.x = q[0] + t_base.transform.rotation.y = q[1] + t_base.transform.rotation.z = q[2] + t_base.transform.rotation.w = q[3] + self.tfbr.sendTransform(t_base) + + def _log_odom_data_callback(self, timestamp, data, logconf, uri): + """ + 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] + + x = data.get('stateEstimate.x') + y = data.get('stateEstimate.y') + z = data.get('stateEstimate.z') + yaw = radians(data.get('stabilizer.yaw')) + roll = radians(data.get('stabilizer.roll')) + pitch = radians(data.get('stabilizer.pitch')) + vx = data.get('kalman.statePX') + vy = data.get('kalman.statePY') + vz = data.get('kalman.statePY') + yawrate = data.get('gyro.z') + rollrate = data.get('gyro.x') + pitchrate = data.get('gyro.y') + + q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) + msg = Odometry() + msg.child_frame_id = cf_name + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self.world_tf_name + msg.pose.pose.position.x = x + msg.pose.pose.position.y = y + msg.pose.pose.position.z = z + msg.pose.pose.orientation.x = q[0] + msg.pose.pose.orientation.y = q[1] + msg.pose.pose.orientation.z = q[2] + msg.pose.pose.orientation.w = q[3] + msg.twist.twist.linear.x = vx + msg.twist.twist.linear.y = vy + msg.twist.twist.linear.z = vz + msg.twist.twist.angular.z = yawrate + msg.twist.twist.angular.y = pitchrate + msg.twist.twist.angular.x = rollrate + + self.swarm._cfs[uri].logging["odom_publisher"].publish(msg) + + t_base = TransformStamped() + t_base.header.stamp = self.get_clock().now().to_msg() + t_base.header.frame_id = 'odom' + t_base.child_frame_id = cf_name + t_base.transform.translation.x = x + t_base.transform.translation.y = y + t_base.transform.translation.z = z + t_base.transform.rotation.x = q[0] + t_base.transform.rotation.y = q[1] + t_base.transform.rotation.z = q[2] + t_base.transform.rotation.w = q[3] + self.tfbr.sendTransform(t_base) + + def _log_custom_data_callback(self, timestamp, data, logconf, uri): + """ + Once custom log block is retrieved from the Crazyflie, + send out the ROS 2 topic for that same type of log + """ + msg = LogDataGeneric() + msg.header.stamp = self.get_clock().now().to_msg() + msg.timestamp = timestamp + for log_name in data: + msg.values.append(data.get(log_name)) + + self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish( + msg) + + def _log_error_callback(self, logconf, msg): + print('Error when logging %s: %s' % (logconf.name, msg)) + + def _init_parameters(self): + """ + 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 + for link_uri in self.uris: + cf = self.swarm._cfs[link_uri].cf + + p_toc = cf.param.toc.toc + + for group in sorted(p_toc.keys()): + for param in sorted(p_toc[group].keys()): + name = group + "." + param + + # Check the parameter type + elem = p_toc[group][param] + type_cf_param = elem.ctype + parameter_descriptor = ParameterDescriptor(type=cf_log_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 + try: + set_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]]["firmware_params"][group][param] + except KeyError: + pass + try: + set_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 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) + self.get_logger().info( + f" {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, + ) + + else: + # If value is not found in initial parameter set + # get crazyflie paramter value and declare that value in ROS 2 parameter + + 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)) + + 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.get_logger().info("All Crazyflies parameters are initialized") + + def _parameters_callback(self, params): + """ + Sets up all the parameters for the crazyflie and + translates it to ROS 2 paraemeters at startup + """ + for param in params: + param_split = param.name.split(".") + + if param_split[0] in self.cf_dict.values(): + cf_name = param_split[0] + if param_split[1] == "params": + name_param = param_split[2] + "." + param_split[3] + try: + self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( + name_param, param.value + ) + self.get_logger().info( + f" {self.uri_dict[cf_name]}: {name_param} is set to {param.value}" + ) + return SetParametersResult(successful=True) + except Exception as e: + self.get_logger().info(str(e)) + return SetParametersResult(successful=False) + if param_split[1] == "logs": + return SetParametersResult(successful=True) + elif param_split[0] == "all": + if param_split[1] == "params": + name_param = param_split[2] + "." + param_split[3] + try: + for link_uri in self.uris: + cf = self.swarm._cfs[link_uri].cf.param.set_value( + name_param, param.value + ) + self.get_logger().info( + f" {link_uri}: {name_param} is set to {param.value}" + ) + return SetParametersResult(successful=True) + except Exception as e: + self.get_logger().info(str(e)) + return SetParametersResult(successful=False) + + return SetParametersResult(successful=False) + + def _emergency_callback(self, request, response, uri="all"): + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.loc.send_emergency_stop() + else: + self.swarm._cfs[uri].cf.loc.send_emergency_stop() + + return response + + def _takeoff_callback(self, request, response, uri="all"): + """ + Service callback to take the crazyflie land to + a certain height in high level commander + """ + + duration = float(request.duration.sec) + \ + float(request.duration.nanosec / 1e9) + self.get_logger().info( + f"takeoff(height={request.height} m," + + f"duration={duration} s," + + f"group_mask={request.group_mask}) {uri}" + ) + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.high_level_commander.takeoff( + request.height, duration + ) + else: + self.swarm._cfs[uri].cf.high_level_commander.takeoff( + request.height, duration + ) + + return response + + def _land_callback(self, request, response, uri="all"): + """ + Service callback to make the crazyflie land to + a certain height in high level commander + """ + duration = float(request.duration.sec) + \ + float(request.duration.nanosec / 1e9) + self.get_logger().info( + f"land(height={request.height} m," + + f"duration={duration} s," + + f"group_mask={request.group_mask})" + ) + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.high_level_commander.land( + request.height, duration, group_mask=request.group_mask + ) + else: + self.swarm._cfs[uri].cf.high_level_commander.land( + request.height, duration, group_mask=request.group_mask + ) + + return response + + def _go_to_callback(self, request, response, uri="all"): + """ + Service callback to have the crazyflie go to + a certain position in high level commander + """ + duration = float(request.duration.sec) + \ + float(request.duration.nanosec / 1e9) + + self.get_logger().info( + "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" + % ( + request.goal.x, + request.goal.y, + request.goal.z, + request.yaw, + duration, + request.relative, + request.group_mask, + ) + ) + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.high_level_commander.go_to( + request.goal.x, + request.goal.y, + request.goal.z, + request.yaw, + duration, + relative=request.relative, + group_mask=request.group_mask, + ) + else: + self.swarm._cfs[uri].cf.high_level_commander.go_to( + request.goal.x, + request.goal.y, + request.goal.z, + request.yaw, + duration, + relative=request.relative, + group_mask=request.group_mask, + ) + return response + + def _notify_setpoints_stop_callback(self, request, response, uri="all"): + self.get_logger().info("Notify setpoint stop not yet implemented") + return response + + def _upload_trajectory_callback(self, request, response, uri="all"): + self.get_logger().info("Notify trajectory not yet implemented") + return response + + def _upload_bezier_trajectory_callback(self, request, response, uri="all"): + id = request.trajectory_id + offset = request.piece_offset + length = len(request.pieces) + total_duration = 0 + + self.get_logger().info( + "upload_trajectory(id=%d, offset=%d, length=%d)" % (id, offset, length) + ) + + + # create the trajectories + # example from cflib + a = 0.9 + b = 0.5 + c = 0.5 + trajectory = [ + CompressedStart(0.0, 0.0, 0.0, 0.0), + CompressedSegment(2.0, [0.0, 1.0, 1.0], [0.0, a, 0.0], [], []), + CompressedSegment(2.0, [1.0, b, 0.0], [-a, -c, 0.0], [], []), + CompressedSegment(2.0, [-b, -1.0, -1.0], [c, a, 0.0], [], []), + CompressedSegment(2.0, [-1.0, 0.0, 0.0], [-a, 0.0, 0.0], [], []), + ] + + trajectory = [] + trajectory.append(CompressedStart(0.0, 0.0, 0.0, 0.0)) + for i in range(length): + piece = request.pieces[i] + duration = float(piece.duration.sec) + \ + float(piece.duration.nanosec)/1e9 + p = CompressedSegment(duration, piece.bezier_control_pts_x, piece.bezier_control_pts_y, piece.bezier_control_pts_z, piece.bezier_control_pts_yaw) + trajectory.append(p) + total_duration += duration + + if uri == "all": + upload_success_all = True + for link_uri in self.uris: + trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ)[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") + upload_success_all = False + else: + self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( + id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) + if upload_success_all is False: + response.success = False + return response + else: + trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( + MemoryElement.TYPE_TRAJ)[id] + trajectory_mem.trajectory = trajectory + upload_result = trajectory_mem.write_data_sync() + if not upload_result: + self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") + response.success = False + return response + self.swarm._cfs[uri].cf.high_level_commander.define_trajectory( + id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) + + return response + + def _start_trajectory_callback(self, request, response, uri="all"): + self.get_logger().info("Start trajectory not yet implemented") + return response + + 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 + """ + + poses = msg.poses + for pose in poses: + name = pose.name + x = pose.pose.position.x + y = pose.pose.position.y + z = pose.pose.position.z + quat = pose.pose.orientation + + if name in self.uri_dict.keys(): + uri = self.uri_dict[name] + # self.get_logger().info(f"{uri}: send extpos {x}, {y}, {z} to {name}") + if isnan(quat.x): + self.swarm._cfs[uri].cf.extpos.send_extpos( + x, y, z) + else: + self.swarm._cfs[uri].cf.extpos.send_extpose( + x, y, z, quat.x, quat.y, quat.z, quat.w) + + def _cmd_vel_legacy_changed(self, msg, uri=""): + """ + Topic update callback to control the attitude and thrust + of the crazyflie with teleop + """ + roll = msg.linear.y + pitch = -msg.linear.x + yawrate = msg.angular.z + thrust = int(min(max(msg.linear.z, 0, 0), 60000)) + self.swarm._cfs[uri].cf.commander.send_setpoint( + roll, pitch, yawrate, thrust) + + def _cmd_hover_changed(self, msg, uri=""): + """ + Topic update callback to control the hover command + of the crazyflie from the velocity multiplexer (vel_mux) + """ + vx = msg.vx + vy = msg.vy + z = msg.z_distance + yawrate = -1.0*degrees(msg.yaw_rate) + self.swarm._cfs[uri].cf.commander.send_hover_setpoint(vx, vy, yawrate, z) + self.get_logger().info(f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") + + def _remove_logging(self, request, response, uri="all"): + """ + Service callback to remove logging blocks of the crazyflie + """ + topic_name = request.topic_name + if topic_name in self.default_log_type.keys(): + try: + self.undeclare_parameter( + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") + self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop() + self.destroy_publisher( + self.swarm._cfs[uri].logging[topic_name + "_publisher"]) + self.get_logger().info(f"{uri}: Remove {topic_name} logging") + except rclpy.exceptions.ParameterNotDeclaredException: + self.get_logger().info( + f"{uri}: No logblock of {topic_name} has been found ") + response.success = False + return response + else: + try: + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"].stop( + ) + for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]: + self.destroy_publisher( + self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name]) + self.get_logger().info(f"{uri}: Remove {topic_name} logging") + except rclpy.exceptions.ParameterNotDeclaredException: + self.get_logger().info( + f"{uri}: No logblock of {topic_name} has been found ") + response.success = False + return response + + response.success = True + return response + + def _add_logging(self, request, response, uri="all"): + """ + Service callback to add logging blocks of the crazyflie + """ + topic_name = request.topic_name + frequency = request.frequency + variables = request.vars + if topic_name in self.default_log_type.keys(): + try: + self.declare_parameter( + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) + self.swarm._cfs[uri].logging[topic_name + "_publisher"] = self.create_publisher( + self.default_log_type[topic_name], self.cf_dict[uri] + "/" + topic_name, 10) + self.swarm._cfs[uri].logging[topic_name + "_log_config"].period_in_ms = 1000 / frequency + self.swarm._cfs[uri].logging[topic_name + "_log_config"].start() + self.get_logger().info(f"{uri}: Add {topic_name} logging") + except rclpy.exceptions.ParameterAlreadyDeclaredException: + self.get_logger().info( + f"{uri}: The content the logging of {topic_name} has already started ") + response.success = False + return response + else: + try: + self.declare_parameter( + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) + self.declare_parameter( + self.cf_dict[uri] + ".logs." + topic_name + ".vars.", variables) + lg_custom = LogConfig( + name=topic_name, period_in_ms=1000 / frequency) + for log_name in variables: + lg_custom.add_variable(log_name) + self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = self.create_publisher( + LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10) + + self.swarm._cfs[uri].cf.log.add_config(lg_custom) + + lg_custom.data_received_cb.add_callback( + partial(self._log_custom_data_callback, uri=uri)) + lg_custom.error_cb.add_callback(self._log_error_callback) + lg_custom.start() + + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name] = {} + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"] = lg_custom + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency + + self.get_logger().info(f"{uri}: Add {topic_name} logging") + except KeyError as e: + self.get_logger().info( + f"{uri}: Failed to add {topic_name} logging") + self.get_logger().info(str(e) + "is not in TOC") + self.undeclare_parameter(self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") + self.undeclare_parameter(self.cf_dict[uri] + ".logs." + topic_name + ".vars.") + response.success = False + return response + except rclpy.exceptions.ParameterAlreadyDeclaredException: + self.get_logger().info( + f"{uri}: The content or part of the logging of {topic_name} has already started ") + response.success = False + return response + + response.success = True + return response + + +def main(args=None): + + cflib.crtp.init_drivers() + rclpy.init(args=args) + crazyflie_server = CrazyflieServer() + + rclpy.spin(crazyflie_server) + + crazyflie_server.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crazyflie_examples/crazyflie_examples/bezier_test.py b/crazyflie_examples/crazyflie_examples/bezier_test.py index aef57ab86..064dbd819 100644 --- a/crazyflie_examples/crazyflie_examples/bezier_test.py +++ b/crazyflie_examples/crazyflie_examples/bezier_test.py @@ -11,6 +11,7 @@ def __init__(self, bezier_curve): control_points = np.array(bezier_curve["control_points"]) self.duration = bezier_curve["parameters"][0] + print(self.duration) self.x = control_points[:, 0] self.y = control_points[:, 1] self.z = control_points[:, 2] @@ -53,7 +54,7 @@ def main(): timeHelper.sleep(4.5) allcfs.startTrajectory(0, timescale=TIMESCALE) - timeHelper.sleep(traj.duration * TIMESCALE + 2.0) + timeHelper.sleep(10 + 2.0) timeHelper.sleep(4.5) allcfs.land(targetHeight=0.0, duration=2.0) From 3b1a2c2bda3058b20a899bb4033f799a79eb9ef3 Mon Sep 17 00:00:00 2001 From: qrthom Date: Sat, 6 Jul 2024 16:41:47 -0400 Subject: [PATCH 04/10] 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," From 0759893ce7b0c67f1e9fa7e83130459dde415cce Mon Sep 17 00:00:00 2001 From: qrthom Date: Sat, 6 Jul 2024 16:51:56 -0400 Subject: [PATCH 05/10] Rename files and add to setup --- .../crazyflie_examples/{bezier_test.py => bezier_example.py} | 0 crazyflie_examples/setup.cfg | 1 + 2 files changed, 1 insertion(+) rename crazyflie_examples/crazyflie_examples/{bezier_test.py => bezier_example.py} (100%) diff --git a/crazyflie_examples/crazyflie_examples/bezier_test.py b/crazyflie_examples/crazyflie_examples/bezier_example.py similarity index 100% rename from crazyflie_examples/crazyflie_examples/bezier_test.py rename to crazyflie_examples/crazyflie_examples/bezier_example.py diff --git a/crazyflie_examples/setup.cfg b/crazyflie_examples/setup.cfg index 72e9b2a0a..69990ea8d 100644 --- a/crazyflie_examples/setup.cfg +++ b/crazyflie_examples/setup.cfg @@ -8,3 +8,4 @@ console_scripts = set_param = crazyflie_examples.set_param:main hello_world4 = crazyflie_examples.hello_world4:main drone_drag = crazyflie_examples.drones_drag.drones_in_drag.Strut.run:main + bezier_example = crazyflie_examples.bezier_example:main From b43ebc4e0591521f14cb608d309f35450b9c9a49 Mon Sep 17 00:00:00 2001 From: Yutong Wang Date: Sun, 7 Jul 2024 17:33:22 -0400 Subject: [PATCH 06/10] Add BezierTrajectory class (user layer) and example --- .../crazyflie_examples/bezier_example.py | 65 ---- .../data/figure8_bezier.json | 25 ++ .../crazyflie_examples/figure8_bezier.py | 39 +++ .../crazyflie_py/bezier_trajectory.py | 320 ++++++++++++++++++ 4 files changed, 384 insertions(+), 65 deletions(-) delete mode 100644 crazyflie_examples/crazyflie_examples/bezier_example.py create mode 100644 crazyflie_examples/crazyflie_examples/data/figure8_bezier.json create mode 100644 crazyflie_examples/crazyflie_examples/figure8_bezier.py create mode 100644 crazyflie_py/crazyflie_py/bezier_trajectory.py diff --git a/crazyflie_examples/crazyflie_examples/bezier_example.py b/crazyflie_examples/crazyflie_examples/bezier_example.py deleted file mode 100644 index 064dbd819..000000000 --- a/crazyflie_examples/crazyflie_examples/bezier_example.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python - -import numpy as np -from pathlib import Path - -from crazyflie_py import * - - -class BezierTrajectoryPiece: - def __init__(self, bezier_curve): - control_points = np.array(bezier_curve["control_points"]) - - self.duration = bezier_curve["parameters"][0] - print(self.duration) - self.x = control_points[:, 0] - self.y = control_points[:, 1] - self.z = control_points[:, 2] - self.yaw = np.zeros_like(self.x) # assume zero yaw for now - - -def main(): - swarm = Crazyswarm() - timeHelper = swarm.timeHelper - allcfs = swarm.allcfs - - # TODO: construct the trajectory - bezier_curve = { - "control_points": [ - [0.0, -1.0, 1.0], - [0.0, -0.9999999999999999, 0.9999999999999999], - [-0.07534305719147254, -0.9943341183411414, 1.000090823889544], - [-0.19988495750934981, -0.9776819294183798, 1.0002323866263076], - [-0.347480864044028, -0.9447239205531746, 1.0003846034356692], - [-0.49199621358744006, -0.890133312602044, 1.0005073942908271], - [-0.6072947829139111, -0.8085840281358084, 1.0005606778422909], - ], - "parameters": [5.0], - } - # Construct the bezier trajectory, ideally this should be done within uploadBezierTrajectory - traj = BezierTrajectoryPiece(bezier_curve) - traj = [traj] - - TRIALS = 1 - TIMESCALE = 1.0 - for i in range(TRIALS): - for cf in allcfs.crazyflies: - cf.uploadBezierTrajectory(0, 0, trajectory=traj) - - allcfs.takeoff(targetHeight=1.0, duration=2.0) - timeHelper.sleep(2.5) - for cf in allcfs.crazyflies: - pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) - cf.goTo(pos, 0, 4.0) - timeHelper.sleep(4.5) - - allcfs.startTrajectory(0, timescale=TIMESCALE) - timeHelper.sleep(10 + 2.0) - - timeHelper.sleep(4.5) - allcfs.land(targetHeight=0.0, duration=2.0) - timeHelper.sleep(3.0) - - -if __name__ == "__main__": - main() diff --git a/crazyflie_examples/crazyflie_examples/data/figure8_bezier.json b/crazyflie_examples/crazyflie_examples/data/figure8_bezier.json new file mode 100644 index 000000000..18df69c92 --- /dev/null +++ b/crazyflie_examples/crazyflie_examples/data/figure8_bezier.json @@ -0,0 +1,25 @@ +{ + "control_points": [ + [ + [0, 0, 1, 1], + [0, 0, 0.9, 0], + [0, 0, 0, 0] + ], + [ + [1, 1, 0.5, 0], + [0, -0.9, -0.5, 0], + [0, 0, 0, 0] + ], + [ + [0, -0.5, -1, -1], + [0, 0.5, 0.9, 0], + [0, 0, 0, 0] + ], + [ + [-1, -1, 0, 0], + [0, -0.9, 0, 0], + [0, 0, 0, 0] + ] + ], + "parameters": [2.0, 2.0, 2.0, 2.0] +} diff --git a/crazyflie_examples/crazyflie_examples/figure8_bezier.py b/crazyflie_examples/crazyflie_examples/figure8_bezier.py new file mode 100644 index 000000000..d30b9b05e --- /dev/null +++ b/crazyflie_examples/crazyflie_examples/figure8_bezier.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python + +import numpy as np +from pathlib import Path + +from crazyflie_py import * + + +def main(): + swarm = Crazyswarm() + timeHelper = swarm.timeHelper + allcfs = swarm.allcfs + + traj = BezierTrajectory() + traj.from_json(Path(__file__).parent / "data/figure8_bezier.json") + + TRIALS = 1 + TIMESCALE = 1.0 + for i in range(TRIALS): + for cf in allcfs.crazyflies: + cf.uploadBezierTrajectory(0, 0, trajectory=traj) + + allcfs.takeoff(targetHeight=1.0, duration=2.0) + timeHelper.sleep(2.5) + for cf in allcfs.crazyflies: + pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) + cf.goTo(pos, 0, 4.0) + timeHelper.sleep(4.5) + + allcfs.startTrajectory(0, timescale=TIMESCALE) + timeHelper.sleep(traj.total_time * TIMESCALE + 2.0) + + timeHelper.sleep(4.5) + allcfs.land(targetHeight=0.0, duration=2.0) + timeHelper.sleep(3.0) + + +if __name__ == "__main__": + main() diff --git a/crazyflie_py/crazyflie_py/bezier_trajectory.py b/crazyflie_py/crazyflie_py/bezier_trajectory.py new file mode 100644 index 000000000..c3a797ecd --- /dev/null +++ b/crazyflie_py/crazyflie_py/bezier_trajectory.py @@ -0,0 +1,320 @@ +#!/usr/bin/env python + +import bezier +import matplotlib.pyplot as plt +import numpy as np +from scipy.special import comb, perm +import math +import json +import random + + +# Single bezier curve class that extends the bezier package +class BezierCurve(bezier.Curve): + def __init__( + self, + nodes, + degree, + ctrl_pts_yaw: np.ndarray = [], + duration=None, + *args, + **kwargs, + ): + super().__init__(nodes, degree, *args, **kwargs) + self.duration = duration if duration else 1.0 + + self.ctrl_pts_x = nodes[0] + self.ctrl_pts_y = nodes[1] + if nodes.shape[0] == 3: + self.ctrl_pts_z = nodes[2] + self.ctrl_pts_yaw = ctrl_pts_yaw + + @classmethod + def from_nodes(cls, nodes: np.ndarray, duration: float = None) -> "BezierCurve": + """Generate a BezierCurve instance from nodes. + + Args: + nodes (np.ndarray): The nodes in the curve, where the columns represent + each node white the rows are the dimension of the ambient space. + duration (float, optional): The duration of the curve. Equivalent as + the maximum parameter value (t) when evaluating the curve. Defaults to None. + + Returns: + BezierCurve: The BezierCurve instance. + """ + _, num_nodes = nodes.shape + degree = num_nodes - 1 + return cls(nodes, degree, duration=duration) + + def derivative(self, k: int = 1): + """Return the k-th derivative of the Bézier curve.""" + if k < 0: + raise ValueError("k can't be negative.") + if k > self.degree: + raise ValueError( + f"k can't be greater than the degree of the curve ({self.degree})." + ) + + nodes = self.nodes + n = self.degree + for _ in range(k): + nodes = (nodes[:, 1:] - nodes[:, :-1]) * n + n -= 1 + return BezierCurve(nodes / self.duration**k, n, self.duration) + + def eval(self, parameter: float, derivative_degree: int = 0) -> np.ndarray: + """Evaluate the Bézier curve at a given parameter. + + Args: + parameter (float): Parameter value to evaluate the curve. Must be in [0, duration]. + derivative_degree (int, optional): The degree of the derivative to evaluate. Defaults to 0. + + Returns: + np.ndarray: The evaluated point in the ambient space. + """ + # construct the bernsteinBasis + basis_functions = self.bernstein_basis( + self.degree, self.duration, parameter, derivative_degree + ) + result = np.zeros((self.dimension,)) + for nodes_idx in range(self.degree + 1): # number of nodes + result += self.nodes[:, nodes_idx] * basis_functions[nodes_idx] + + return result + + @staticmethod + def bernstein_basis(bezier_degree, max_parameter, parameter, derivative_degree): + if parameter < 0 or parameter > max_parameter: + raise ValueError( + f"bernsteinBasis: given parameter is outside of [0, {max_parameter}]." + ) + + result = np.zeros(bezier_degree + 1) + + if max_parameter == 0: + if derivative_degree == 0: + result[0] = 1.0 + return result + + for i in range(bezier_degree + 1): + base = 0.0 + mult = 1.0 + for j in range(bezier_degree - derivative_degree + 1): + if j + derivative_degree >= i: + comb_result = comb(bezier_degree - i, j + derivative_degree - i) + perm_result = perm(j + derivative_degree, derivative_degree) + pow_result = np.power(1.0 / max_parameter, j + derivative_degree) + base += ( + comb_result + * pow_result + * perm_result + * mult + * ((j + derivative_degree - i) % 2 == 0 and 1 or -1) + ) + mult *= parameter + + comb_result = comb(bezier_degree, i) + base *= comb_result + result[i] = base + + return result + + # Overrides the method defined in the Bezier package + def plot( + self, + num_points=200, + color="blue", + show_control_points=True, + control_color="black", + ax=None, + **kwargs, + ): + """Plot the Bézier curve. Currently supports 2D and 3D curves. + + Args: + num_points (int, optional): Number of points to evaluate the curve. Defaults to 200. + color (str, optional): Color of the curve. Defaults to "blue". + show_control_points (bool, optional): Whether to show control points in the plot. Defaults to True. + control_color (str, optional): Color of the control points. Defaults to "black". + ax (optional): The matplotlib axis to plot the curve on. If None, will create + a new axis. Otherwise will use the provided axis. Defaults to None. + + Raises: + ValueError: If the dimension of the curve is not 2 or 3. + """ + if self.dimension not in [2, 3]: + raise ValueError(f"Unsupported dimension: {self.dimension}") + + t = np.linspace(0, self.duration, int(num_points * self.duration)) + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d" if self.dimension == 3 else None) + + # ax.set_aspect("equal", "datalim") + points = np.array([self.eval(ti, 0) for ti in t]).transpose() + ax.plot(*points[: self.dimension], color=color, **kwargs) + + if show_control_points: + ax.plot(*self.nodes[: self.dimension], "o--", color=control_color) + + +# Note: BezierTrajectory currently doesn't support yaw control points +class BezierTrajectory: + def __init__(self, curve_list: list[BezierCurve]): + self.curve_list = curve_list + self.num_pieces = len(curve_list) + self.control_points_list = [curve.nodes for curve in curve_list] + self.duration_list = [curve.duration for curve in curve_list] + self.total_time = sum(self.duration_list) + + @classmethod + def from_control_points( + cls, control_points_list: list[list[list[float]]], duration_list=None + ): + curve_list = [] + for i in range(len(control_points_list)): + try: + control_points = np.array(control_points_list[i]) + except ValueError: + raise ValueError(f"Piece {i} is invalid. Check for dimension mismatch.") + if i > 0 and control_points.shape[0] != curve_list[-1].dimension: + raise ValueError( + f"Piece {i} has a different dimension than the previous piece." + ) + max_param = duration_list[i] if duration_list is not None else None + curve_list.append(BezierCurve.from_nodes(control_points, max_param)) + return cls(curve_list) + + @classmethod + def from_json(cls, json_file: str): + f = open(json_file) + json_object = json.load(f) + control_points_list = json_object.get("control_points", None) + if control_points_list is None: + raise ValueError("control_points key is missing in the JSON file.") + parameter_list = json_object.get("parameters", None) + return cls.from_control_points(control_points_list, parameter_list) + + def eval(self, parameter: float, derivative_degree: int = 0): + if parameter < 0 or parameter > self.total_time: + raise ValueError( + f"Parameter {parameter} is outside of [0, {self.total_time}]." + ) + + piece_idx = 0 + tolerance = 1e-10 # Define a small tolerance value + while parameter - self.duration_list[piece_idx] > tolerance: + parameter -= self.duration_list[piece_idx] + parameter = math.floor(parameter * 10**10) / 10**10 + piece_idx += 1 + + return self.curve_list[piece_idx].eval( + parameter, derivative_degree=derivative_degree + ) + + def visualize( + self, + derivatives=None, + show_plot=True, + show_gradient=False, + resolution=20, + ax=None, + **kwargs, + ): + """Visualizes the piecewise Bézier curve. + + Args: + derivatives (list, optional): List of derivatives to visualize. Defaults to None. + show_plot (bool, optional): Whether to display the plot. Defaults to True. Set to False if you + want to use this function as a helper and display the plot later. + color (tuple, optional): Color of the curve in tuples of three floats in [0, 1]. Uses a random color + if not provided. Defaults to None (will use random color). + show_gradient (bool, optional): Whether to show the color gradient. + Showing the color gradient makes the rendering much slower. Defaults to False. + resolution (int, optional): Number of points per 1 in t. Defaults to 20. + Increasing the resolution will make the gradient smoother, but will also increase the rendering time. + **kwargs: Additional keyword arguments to pass to the plot function. + + """ + color = kwargs.get("color", "blue") + label = kwargs.get("label", None) + if derivatives is None: + derivatives = [0] + dim = self.curve_list[0].dimension + + for d in derivatives: + if ax is None: + fig = plt.figure() + axis = fig.add_subplot(111, projection="3d" if dim == 3 else None) + else: + axis = ax + + # change this parameter to increase/decrease resolution + points_count = int(resolution * self.total_time) + t = np.linspace(0, self.total_time, points_count) + t[-1] = self.total_time + points = np.array([self.eval(ti, d) for ti in t]).transpose() + + if show_gradient: + if len(color) == 4: # if color is RGBA + color = color[:3] + + alphas = np.linspace(0.1, 1, points_count - 1) + + for i in range(points_count - 1): + axis.plot( + *points[:dim, i : i + 2], + color=color, + alpha=alphas[i], + ) + + else: + axis.plot(*points[:dim], color=color, label=label) + + title = ( + f"{d}-th derivative for Bézier trajectory" + if d != 0 + else "Original Bézier trajectory" + ) + axis.set_title(title) + + if show_plot: + plt.show() + plt.close(fig) + axis = ax + + +if __name__ == "__main__": + # Example usage + + a = 0.9 + b = 0.5 + c = 0.5 + + # Define the control points for each piece of the trajectory + + # BezierCurve 1 with control points at (0, 0, 0), (0, 0, 0), (1, a, 0), (1, 0, 0) + control_pts1 = [[0, 0, 1, 1], [0, 0, a, 0], [0, 0, 0, 0]] + + # Bezier Curve 2 with control points at (1, 0, 0), (1, -a, 0), (b, -c, 0), (0, 0, 0) + control_pts2 = [[1, 1, b, 0], [0, -a, -c, 0], [0, 0, 0, 0]] + + # Bezier Curve 3 with control points at (0, 0, 0), (-b, c, 0), (-1, a, 0), (-1, 0, 0) + control_pts3 = [[0, -b, -1, -1], [0, c, a, 0], [0, 0, 0, 0]] + + # Bezier Curve 4 with control points at (-1, 0, 0), (-1, -a, 0), (0, 0, 0), (0, 0, 0) + control_pts4 = [[-1, -1, 0, 0], [0, -a, 0, 0], [0, 0, 0, 0]] + + # Plot one curve + curve = BezierCurve.from_nodes(np.array(control_pts1)) + curve.plot() + plt.show() + + # Plot the entire trajectory + control_pts_list = [control_pts1, control_pts2, control_pts3, control_pts4] + bezier_traj = BezierTrajectory.from_control_points(control_pts_list) + # Alternatively, load from json + # bezier_traj = BezierTrajectory.from_json( + # "crazyflie_examples/crazyflie_examples/data/figure8_bezier.json" + # ) + bezier_traj.visualize(derivatives=[0], show_plot=True) From ec0a71f54dcc087146353ad80e535f9ac9c77bfb Mon Sep 17 00:00:00 2001 From: Yutong Wang Date: Sun, 7 Jul 2024 17:33:49 -0400 Subject: [PATCH 07/10] Update cflib backend BezierTrajectory server callback and client method --- crazyflie/scripts/crazyflie_server.py | 179 ++-- crazyflie/scripts/old_crazyflie_server.py | 1032 --------------------- crazyflie_py/crazyflie_py/crazyflie.py | 33 +- 3 files changed, 138 insertions(+), 1106 deletions(-) delete mode 100644 crazyflie/scripts/old_crazyflie_server.py diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index a11450500..0da64a24a 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -62,6 +62,7 @@ "double": ParameterType.PARAMETER_DOUBLE, } + class CrazyflieServer(Node): def __init__(self): super().__init__( @@ -78,21 +79,43 @@ 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']} - 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: @@ -109,7 +132,9 @@ 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) @@ -121,7 +146,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: @@ -129,8 +154,7 @@ 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 ) @@ -143,15 +167,21 @@ 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 @@ -162,26 +192,36 @@ 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 @@ -195,17 +235,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: @@ -213,8 +262,10 @@ 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: @@ -296,7 +347,9 @@ 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 """ @@ -306,26 +359,28 @@ def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_ 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") @@ -336,8 +391,11 @@ def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_ 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" @@ -348,8 +406,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, {}) @@ -945,15 +1003,17 @@ def _upload_trajectory_callback(self, request, response, uri="all"): def _upload_bezier_trajectory_callback(self, request, response, uri="all"): id = request.trajectory_id offset = request.piece_offset - length = len(request.pieces) + num_pieces = len(request.pieces) total_duration = 0 self.get_logger().info( - "upload_trajectory(id=%d, offset=%d, length=%d)" % (id, offset, length) + "upload_trajectory(id=%d, offset=%d, num_pieces=%d)" + % (id, offset, num_pieces) ) # create the trajectories # example from cflib + # TODO: test if yaw is 0 is the same as empty a = 0.9 b = 0.5 c = 0.5 @@ -966,16 +1026,20 @@ def _upload_bezier_trajectory_callback(self, request, response, uri="all"): ] # trajectory = [] - # trajectory.append(CompressedStart(0.0, 0.0, 0.0, 0.0)) - # for i in range(length): + # start_x = request.pieces[0].bezier_control_pts_x[0] + # start_y = request.pieces[0].bezier_control_pts_y[0] + # start_z = request.pieces[0].bezier_control_pts_z[0] + # start_yaw = 0.0 + # trajectory.append(CompressedStart(start_x, start_y, start_z, start_yaw)) + # for i in range(num_pieces): # piece = request.pieces[i] # duration = float(piece.duration.sec) + float(piece.duration.nanosec) / 1e9 # p = CompressedSegment( # duration, - # piece.bezier_control_pts_x, - # piece.bezier_control_pts_y, - # piece.bezier_control_pts_z, - # piece.bezier_control_pts_yaw, + # piece.bezier_control_pts_x[1:], + # piece.bezier_control_pts_y[1:], + # piece.bezier_control_pts_z[1:], + # piece.bezier_control_pts_yaw[1:], # ) # trajectory.append(p) # total_duration += duration @@ -994,9 +1058,6 @@ def _upload_bezier_trajectory_callback(self, request, response, uri="all"): ) upload_success_all = False else: - self.get_logger().info( - f"[{self.cf_dict[uri]}] Upload bezier trajectory success" - ) self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( id, offset, diff --git a/crazyflie/scripts/old_crazyflie_server.py b/crazyflie/scripts/old_crazyflie_server.py deleted file mode 100644 index d769a7650..000000000 --- a/crazyflie/scripts/old_crazyflie_server.py +++ /dev/null @@ -1,1032 +0,0 @@ -#!/usr/bin/env python3 - -""" -A crazyflie server for communicating with several crazyflies - based on the official crazyflie python library from - Bitcraze AB - - - 2022 - K. N. McGuire (Bitcraze AB) -""" - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy -from rclpy.duration import Duration - -import time - -import cflib.crtp -from cflib.crazyflie.swarm import CachedCfFactory -from cflib.crazyflie.swarm import Swarm -from cflib.crazyflie.mem import MemoryElement -from cflib.crazyflie.mem import CompressedSegment -from cflib.crazyflie.mem import CompressedStart -from cflib.crazyflie.log import LogConfig -from cflib.crazyflie.high_level_commander import HighLevelCommander - -from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging -from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop, UploadBezierTrajectory -from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType -from crazyflie_interfaces.msg import Hover -from crazyflie_interfaces.msg import LogDataGeneric -from motion_capture_tracking_interfaces.msg import NamedPoseArray - -from std_srvs.srv import Empty -from geometry_msgs.msg import Twist -from geometry_msgs.msg import PoseStamped, TransformStamped -from sensor_msgs.msg import LaserScan -from nav_msgs.msg import Odometry - -import tf_transformations -from tf2_ros import TransformBroadcaster - -from functools import partial -from math import degrees, radians, pi, isnan - -cf_log_to_ros_param = { - "uint8_t": ParameterType.PARAMETER_INTEGER, - "uint16_t": ParameterType.PARAMETER_INTEGER, - "uint32_t": ParameterType.PARAMETER_INTEGER, - "int8_t": ParameterType.PARAMETER_INTEGER, - "int16_t": ParameterType.PARAMETER_INTEGER, - "int32_t": ParameterType.PARAMETER_INTEGER, - "FP16": ParameterType.PARAMETER_DOUBLE, - "float": ParameterType.PARAMETER_DOUBLE, - "double": ParameterType.PARAMETER_DOUBLE, -} - -class CrazyflieServer(Node): - def __init__(self): - super().__init__( - "crazyflie_server", - allow_undeclared_parameters=True, - automatically_declare_parameters_from_overrides=True, - ) - - # Turn ROS parameters into a dictionary - self._ros_parameters = self._param_to_dict(self._parameters) - - self.uris = [] - self.cf_dict = {} - 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']} - 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: - self.world_tf_name = self._ros_parameters["world_tf_name"] - except KeyError: - pass - robot_data = self._ros_parameters["robots"] - - # Init a transform broadcaster - self.tfbr = TransformBroadcaster(self) - - # Create easy lookup tables for uri, name and types - for crazyflie in robot_data: - 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") - if connection == "crazyflie": - uri = robot_data[crazyflie]["uri"] - self.uris.append(uri) - self.cf_dict[uri] = crazyflie - self.uri_dict[crazyflie] = uri - self.type_dict[uri] = type_cf - - # Setup Swarm class cflib with connection callbacks and open the links - 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.connection_failed.add_callback( - self._connection_failed - ) - - self.swarm._cfs[link_uri].logging = {} - - cf_name = self.cf_dict[link_uri] - cf_type = self.type_dict[link_uri] - - # check if logging is enabled at startup - logging_enabled = False - try: - 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"] - except KeyError: - pass - try: - logging_enabled = self._ros_parameters['robots'][cf_name]["firmware_logging"]["enabled"] - except KeyError: - pass - - self.swarm._cfs[link_uri].logging["enabled"] = logging_enabled - - # check if predefine log blocks can be logged and setup crazyflie logblocks and ROS 2 publishers - for default_log_name in self.default_log_type: - 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) - - # 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_logging_enabled = True - except KeyError: - pass - try: - custom_log_topics.update( - 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"]) - custom_logging_enabled = True - except KeyError: - pass - - self.swarm._cfs[link_uri].logging["custom_log_topics"] = {} - self.swarm._cfs[link_uri].logging["custom_log_groups"] = {} - self.swarm._cfs[link_uri].logging["custom_log_publisher"] = {} - - # Setup log blocks for each custom log and ROS 2 publisher topics - if custom_logging_enabled: - 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) - 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"] - - # 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") - exit() - - # Create services for the entire swarm and each individual crazyflie - self.create_service(Empty, "all/emergency", self._emergency_callback) - self.create_service(Takeoff, "all/takeoff", self._takeoff_callback) - self.create_service(Land, "all/land", self._land_callback) - self.create_service(GoTo, "all/go_to", self._go_to_callback) - self.create_service(StartTrajectory, "all/start_trajectory", self._start_trajectory_callback) - - for uri in self.cf_dict: - name = self.cf_dict[uri] - self.create_service( - Empty, name + - "/emergency", partial(self._emergency_callback, uri=uri) - ) - self.create_service( - Takeoff, name + - "/takeoff", partial(self._takeoff_callback, uri=uri) - ) - self.create_service( - Land, name + "/land", partial(self._land_callback, uri=uri) - ) - self.create_service( - GoTo, name + "/go_to", partial(self._go_to_callback, uri=uri) - ) - self.create_service( - StartTrajectory, name + "/start_trajectory", partial(self._start_trajectory_callback, uri=uri) - ) - self.create_service( - UploadTrajectory, name + "/upload_trajectory", partial(self._upload_trajectory_callback, uri=uri) - ) - self.create_service( - UploadBezierTrajectory, name + "/upload_bezier_trajectory", partial(self._upload_bezier_trajectory_callback, uri=uri) - ) - self.create_service( - NotifySetpointsStop, name + "/notify_setpoints_stop", partial(self._notify_setpoints_stop_callback, uri=uri) - ) - self.create_subscription( - Twist, name + - "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, uri=uri), 10 - ) - self.create_subscription( - Hover, name + - "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 - ) - qos_profile = QoSProfile(reliability =QoSReliabilityPolicy.BEST_EFFORT, - history=QoSHistoryPolicy.KEEP_LAST, - depth=1, - deadline = Duration(seconds=0, nanoseconds=1e9/100.0)) - self.create_subscription( - NamedPoseArray, "/poses", - self._poses_changed, qos_profile - ) - - def _init_default_logblocks(self, prefix, link_uri, list_logvar, global_logging_enabled, topic_type): - """ - Prepare default logblocks as defined in crazyflies.yaml - """ - cf_name = self.cf_dict[link_uri] - cf_type = self.type_dict[link_uri] - - logging_enabled = False - logging_freq = 10 - try: - 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_enabled = True - except KeyError: - pass - try: - 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) - for logvar in list_logvar: - if prefix == "odom": - lg.add_variable(logvar, "FP16") - else: - lg.add_variable(logvar) - - self.swarm._cfs[link_uri].logging[prefix + "_logging_enabled"] = logging_enabled - 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) - else: - self.swarm._cfs[link_uri].logging[prefix + "_publisher"] = "empty" - - def _param_to_dict(self, param_ros): - """ - Turn ROS 2 parameters from the node into a dict - """ - tree = {} - for item in param_ros: - t = tree - for part in item.split('.'): - if part == item.split('.')[-1]: - t = t.setdefault(part, param_ros[item].value) - else: - t = t.setdefault(part, {}) - return tree - - def _fully_connected(self, link_uri): - """ - Called when all parameters have been updated - and the full log toc has been received of the Crazyflie - """ - self.get_logger().info(f" {link_uri} is fully connected!") - - self.swarm.fully_connected_crazyflie_cnt += 1 - - if self.swarm.fully_connected_crazyflie_cnt == len(self.cf_dict): - self.get_logger().info("All Crazyflies are fully connected!") - self._init_parameters() - self._init_logging() - self.add_on_set_parameters_callback(self._parameters_callback) - else: - return - - def _disconnected(self, link_uri): - self.get_logger().info(f" {link_uri} is disconnected!") - - def _connection_failed(self, link_uri, msg): - self.get_logger().info(f"{link_uri} connection Failed") - self.swarm.close_links() - - def _init_logging(self): - """ - Sets up all the log blocks for the crazyflie and - all the ROS 2 publisher and parameters for logging - at startup - """ - for link_uri in self.uris: - cf_handle = self.swarm._cfs[link_uri] - cf = cf_handle.cf - - # Start logging for predefined logging - for default_log_name in self.default_log_type: - prefix = default_log_name - if cf_handle.logging[prefix + "_logging_enabled"] and cf_handle.logging["enabled"]: - callback_fnc = self.default_log_fnc[prefix] - self._init_default_logging(prefix, link_uri, callback_fnc) - - # Start logging for costum logging blocks - cf_handle.l_toc = cf.log.toc.toc - if len(cf_handle.logging["custom_log_groups"]) != 0 and cf_handle.logging["enabled"]: - - for log_group_name, log_group_dict in cf_handle.logging["custom_log_groups"].items(): - self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = self.create_publisher( - LogDataGeneric, self.cf_dict[link_uri] + "/" + log_group_name, 10) - lg_custom = log_group_dict['log_config'] - try: - cf.log.add_config(lg_custom) - lg_custom.data_received_cb.add_callback( - partial(self._log_custom_data_callback, uri=link_uri)) - lg_custom.error_cb.add_callback( - self._log_error_callback) - lg_custom.start() - except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') - - self.get_logger().info(f"{link_uri} setup custom logging") - - self.create_service( - RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri)) - self.create_service( - AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri)) - - self.get_logger().info("All Crazyflies loggging are initialized") - - def _init_default_logging(self, prefix, link_uri, callback_fnc): - """ - Sets up all the default log blocks and ROS 2 publishers for the crazyflie - """ - cf_handle = self.swarm._cfs[link_uri] - cf = cf_handle.cf - lg = cf_handle.logging[prefix + "_log_config"] - try: - cf.log.add_config(lg) - lg.data_received_cb.add_callback( - partial(callback_fnc, uri=link_uri)) - lg.error_cb.add_callback(self._log_error_callback) - lg.start() - frequency = cf_handle.logging[prefix + "_logging_freq"] - self.declare_parameter( - self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency) - self.get_logger().info( - f"{link_uri} setup logging for {prefix} at freq {frequency}") - except KeyError as e: - self.get_logger().info(f'{link_uri}: Could not start log configuration,' - '{} not found in TOC'.format(str(e))) - except AttributeError: - self.get_logger().info( - f'{link_uri}: Could not add log config, bad configuration.') - - def _log_scan_data_callback(self, timestamp, data, logconf, uri): - """ - Once multiranger range is retrieved from the Crazyflie, - send out the ROS 2 topic for Scan - """ - cf_name = self.cf_dict[uri] - max_range = 3.49 - front_range = float(data.get('range.front'))/1000.0 - left_range = float(data.get('range.left'))/1000.0 - back_range = float(data.get('range.back'))/1000.0 - right_range = float(data.get('range.right'))/1000.0 - if front_range > max_range: - front_range = float("inf") - if left_range > max_range: - left_range = float("inf") - if right_range > max_range: - right_range = float("inf") - if back_range > max_range: - back_range = float("inf") - self.ranges = [back_range, right_range, front_range, left_range] - - msg = LaserScan() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = cf_name - msg.range_min = 0.01 - msg.range_max = 3.49 - msg.ranges = self.ranges - msg.angle_min = -0.5 * 2* pi - msg.angle_max = 0.25 * 2 * pi - msg.angle_increment = 1.0 * pi/2 - self.swarm._cfs[uri].logging["scan_publisher"].publish(msg) - - def _log_pose_data_callback(self, timestamp, data, logconf, uri): - """ - Once pose data is retrieved from the Crazyflie, - send out the ROS 2 topic for Pose - """ - - cf_name = self.cf_dict[uri] - - x = data.get('stateEstimate.x') - y = data.get('stateEstimate.y') - z = data.get('stateEstimate.z') - roll = radians(data.get('stabilizer.roll')) - pitch = radians(-1.0 * data.get('stabilizer.pitch')) - yaw = radians(data.get('stabilizer.yaw')) - q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) - - msg = PoseStamped() - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = self.world_tf_name - msg.pose.position.x = x - msg.pose.position.y = y - msg.pose.position.z = z - msg.pose.orientation.x = q[0] - msg.pose.orientation.y = q[1] - msg.pose.orientation.z = q[2] - msg.pose.orientation.w = q[3] - self.swarm._cfs[uri].logging["pose_publisher"].publish(msg) - - t_base = TransformStamped() - t_base.header.stamp = self.get_clock().now().to_msg() - t_base.header.frame_id = self.world_tf_name - t_base.child_frame_id = cf_name - t_base.transform.translation.x = x - t_base.transform.translation.y = y - t_base.transform.translation.z = z - t_base.transform.rotation.x = q[0] - t_base.transform.rotation.y = q[1] - t_base.transform.rotation.z = q[2] - t_base.transform.rotation.w = q[3] - self.tfbr.sendTransform(t_base) - - def _log_odom_data_callback(self, timestamp, data, logconf, uri): - """ - 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] - - x = data.get('stateEstimate.x') - y = data.get('stateEstimate.y') - z = data.get('stateEstimate.z') - yaw = radians(data.get('stabilizer.yaw')) - roll = radians(data.get('stabilizer.roll')) - pitch = radians(data.get('stabilizer.pitch')) - vx = data.get('kalman.statePX') - vy = data.get('kalman.statePY') - vz = data.get('kalman.statePY') - yawrate = data.get('gyro.z') - rollrate = data.get('gyro.x') - pitchrate = data.get('gyro.y') - - q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) - msg = Odometry() - msg.child_frame_id = cf_name - msg.header.stamp = self.get_clock().now().to_msg() - msg.header.frame_id = self.world_tf_name - msg.pose.pose.position.x = x - msg.pose.pose.position.y = y - msg.pose.pose.position.z = z - msg.pose.pose.orientation.x = q[0] - msg.pose.pose.orientation.y = q[1] - msg.pose.pose.orientation.z = q[2] - msg.pose.pose.orientation.w = q[3] - msg.twist.twist.linear.x = vx - msg.twist.twist.linear.y = vy - msg.twist.twist.linear.z = vz - msg.twist.twist.angular.z = yawrate - msg.twist.twist.angular.y = pitchrate - msg.twist.twist.angular.x = rollrate - - self.swarm._cfs[uri].logging["odom_publisher"].publish(msg) - - t_base = TransformStamped() - t_base.header.stamp = self.get_clock().now().to_msg() - t_base.header.frame_id = 'odom' - t_base.child_frame_id = cf_name - t_base.transform.translation.x = x - t_base.transform.translation.y = y - t_base.transform.translation.z = z - t_base.transform.rotation.x = q[0] - t_base.transform.rotation.y = q[1] - t_base.transform.rotation.z = q[2] - t_base.transform.rotation.w = q[3] - self.tfbr.sendTransform(t_base) - - def _log_custom_data_callback(self, timestamp, data, logconf, uri): - """ - Once custom log block is retrieved from the Crazyflie, - send out the ROS 2 topic for that same type of log - """ - msg = LogDataGeneric() - msg.header.stamp = self.get_clock().now().to_msg() - msg.timestamp = timestamp - for log_name in data: - msg.values.append(data.get(log_name)) - - self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish( - msg) - - def _log_error_callback(self, logconf, msg): - print('Error when logging %s: %s' % (logconf.name, msg)) - - def _init_parameters(self): - """ - 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 - for link_uri in self.uris: - cf = self.swarm._cfs[link_uri].cf - - p_toc = cf.param.toc.toc - - for group in sorted(p_toc.keys()): - for param in sorted(p_toc[group].keys()): - name = group + "." + param - - # Check the parameter type - elem = p_toc[group][param] - type_cf_param = elem.ctype - parameter_descriptor = ParameterDescriptor(type=cf_log_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 - try: - set_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]]["firmware_params"][group][param] - except KeyError: - pass - try: - set_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 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) - self.get_logger().info( - f" {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, - ) - - else: - # If value is not found in initial parameter set - # get crazyflie paramter value and declare that value in ROS 2 parameter - - 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)) - - 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.get_logger().info("All Crazyflies parameters are initialized") - - def _parameters_callback(self, params): - """ - Sets up all the parameters for the crazyflie and - translates it to ROS 2 paraemeters at startup - """ - for param in params: - param_split = param.name.split(".") - - if param_split[0] in self.cf_dict.values(): - cf_name = param_split[0] - if param_split[1] == "params": - name_param = param_split[2] + "." + param_split[3] - try: - self.swarm._cfs[self.uri_dict[cf_name]].cf.param.set_value( - name_param, param.value - ) - self.get_logger().info( - f" {self.uri_dict[cf_name]}: {name_param} is set to {param.value}" - ) - return SetParametersResult(successful=True) - except Exception as e: - self.get_logger().info(str(e)) - return SetParametersResult(successful=False) - if param_split[1] == "logs": - return SetParametersResult(successful=True) - elif param_split[0] == "all": - if param_split[1] == "params": - name_param = param_split[2] + "." + param_split[3] - try: - for link_uri in self.uris: - cf = self.swarm._cfs[link_uri].cf.param.set_value( - name_param, param.value - ) - self.get_logger().info( - f" {link_uri}: {name_param} is set to {param.value}" - ) - return SetParametersResult(successful=True) - except Exception as e: - self.get_logger().info(str(e)) - return SetParametersResult(successful=False) - - return SetParametersResult(successful=False) - - def _emergency_callback(self, request, response, uri="all"): - if uri == "all": - for link_uri in self.uris: - self.swarm._cfs[link_uri].cf.loc.send_emergency_stop() - else: - self.swarm._cfs[uri].cf.loc.send_emergency_stop() - - return response - - def _takeoff_callback(self, request, response, uri="all"): - """ - Service callback to take the crazyflie land to - a certain height in high level commander - """ - - duration = float(request.duration.sec) + \ - float(request.duration.nanosec / 1e9) - self.get_logger().info( - f"takeoff(height={request.height} m," - + f"duration={duration} s," - + f"group_mask={request.group_mask}) {uri}" - ) - if uri == "all": - for link_uri in self.uris: - self.swarm._cfs[link_uri].cf.high_level_commander.takeoff( - request.height, duration - ) - else: - self.swarm._cfs[uri].cf.high_level_commander.takeoff( - request.height, duration - ) - - return response - - def _land_callback(self, request, response, uri="all"): - """ - Service callback to make the crazyflie land to - a certain height in high level commander - """ - duration = float(request.duration.sec) + \ - float(request.duration.nanosec / 1e9) - self.get_logger().info( - f"land(height={request.height} m," - + f"duration={duration} s," - + f"group_mask={request.group_mask})" - ) - if uri == "all": - for link_uri in self.uris: - self.swarm._cfs[link_uri].cf.high_level_commander.land( - request.height, duration, group_mask=request.group_mask - ) - else: - self.swarm._cfs[uri].cf.high_level_commander.land( - request.height, duration, group_mask=request.group_mask - ) - - return response - - def _go_to_callback(self, request, response, uri="all"): - """ - Service callback to have the crazyflie go to - a certain position in high level commander - """ - duration = float(request.duration.sec) + \ - float(request.duration.nanosec / 1e9) - - self.get_logger().info( - "go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" - % ( - request.goal.x, - request.goal.y, - request.goal.z, - request.yaw, - duration, - request.relative, - request.group_mask, - ) - ) - if uri == "all": - for link_uri in self.uris: - self.swarm._cfs[link_uri].cf.high_level_commander.go_to( - request.goal.x, - request.goal.y, - request.goal.z, - request.yaw, - duration, - relative=request.relative, - group_mask=request.group_mask, - ) - else: - self.swarm._cfs[uri].cf.high_level_commander.go_to( - request.goal.x, - request.goal.y, - request.goal.z, - request.yaw, - duration, - relative=request.relative, - group_mask=request.group_mask, - ) - return response - - def _notify_setpoints_stop_callback(self, request, response, uri="all"): - self.get_logger().info("Notify setpoint stop not yet implemented") - return response - - def _upload_trajectory_callback(self, request, response, uri="all"): - self.get_logger().info("Notify trajectory not yet implemented") - return response - - def _upload_bezier_trajectory_callback(self, request, response, uri="all"): - id = request.trajectory_id - offset = request.piece_offset - length = len(request.pieces) - total_duration = 0 - - self.get_logger().info( - "upload_trajectory(id=%d, offset=%d, length=%d)" % (id, offset, length) - ) - - - # create the trajectories - # example from cflib - a = 0.9 - b = 0.5 - c = 0.5 - trajectory = [ - CompressedStart(0.0, 0.0, 0.0, 0.0), - CompressedSegment(2.0, [0.0, 1.0, 1.0], [0.0, a, 0.0], [], []), - CompressedSegment(2.0, [1.0, b, 0.0], [-a, -c, 0.0], [], []), - CompressedSegment(2.0, [-b, -1.0, -1.0], [c, a, 0.0], [], []), - CompressedSegment(2.0, [-1.0, 0.0, 0.0], [-a, 0.0, 0.0], [], []), - ] - - trajectory = [] - trajectory.append(CompressedStart(0.0, 0.0, 0.0, 0.0)) - for i in range(length): - piece = request.pieces[i] - duration = float(piece.duration.sec) + \ - float(piece.duration.nanosec)/1e9 - p = CompressedSegment(duration, piece.bezier_control_pts_x, piece.bezier_control_pts_y, piece.bezier_control_pts_z, piece.bezier_control_pts_yaw) - trajectory.append(p) - total_duration += duration - - if uri == "all": - upload_success_all = True - for link_uri in self.uris: - trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( - MemoryElement.TYPE_TRAJ)[id] - trajectory_mem.trajectory = trajectory - upload_result = trajectory_mem.write_data_sync() - if not upload_result: - self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") - upload_success_all = False - else: - self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( - id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) - if upload_success_all is False: - response.success = False - return response - else: - trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( - MemoryElement.TYPE_TRAJ)[id] - trajectory_mem.trajectory = trajectory - upload_result = trajectory_mem.write_data_sync() - if not upload_result: - self.get_logger().info(f"[{self.cf_dict[uri]}] Upload bezier trajectory failed") - response.success = False - return response - self.swarm._cfs[uri].cf.high_level_commander.define_trajectory( - id, offset, len(trajectory), type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED) - - return response - - def _start_trajectory_callback(self, request, response, uri="all"): - self.get_logger().info("Start trajectory not yet implemented") - return response - - 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 - """ - - poses = msg.poses - for pose in poses: - name = pose.name - x = pose.pose.position.x - y = pose.pose.position.y - z = pose.pose.position.z - quat = pose.pose.orientation - - if name in self.uri_dict.keys(): - uri = self.uri_dict[name] - # self.get_logger().info(f"{uri}: send extpos {x}, {y}, {z} to {name}") - if isnan(quat.x): - self.swarm._cfs[uri].cf.extpos.send_extpos( - x, y, z) - else: - self.swarm._cfs[uri].cf.extpos.send_extpose( - x, y, z, quat.x, quat.y, quat.z, quat.w) - - def _cmd_vel_legacy_changed(self, msg, uri=""): - """ - Topic update callback to control the attitude and thrust - of the crazyflie with teleop - """ - roll = msg.linear.y - pitch = -msg.linear.x - yawrate = msg.angular.z - thrust = int(min(max(msg.linear.z, 0, 0), 60000)) - self.swarm._cfs[uri].cf.commander.send_setpoint( - roll, pitch, yawrate, thrust) - - def _cmd_hover_changed(self, msg, uri=""): - """ - Topic update callback to control the hover command - of the crazyflie from the velocity multiplexer (vel_mux) - """ - vx = msg.vx - vy = msg.vy - z = msg.z_distance - yawrate = -1.0*degrees(msg.yaw_rate) - self.swarm._cfs[uri].cf.commander.send_hover_setpoint(vx, vy, yawrate, z) - self.get_logger().info(f"{uri}: Received hover topic {vx} {vy} {yawrate} {z}") - - def _remove_logging(self, request, response, uri="all"): - """ - Service callback to remove logging blocks of the crazyflie - """ - topic_name = request.topic_name - if topic_name in self.default_log_type.keys(): - try: - self.undeclare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") - self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop() - self.destroy_publisher( - self.swarm._cfs[uri].logging[topic_name + "_publisher"]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") - except rclpy.exceptions.ParameterNotDeclaredException: - self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") - response.success = False - return response - else: - try: - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"].stop( - ) - for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]: - self.destroy_publisher( - self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name]) - self.get_logger().info(f"{uri}: Remove {topic_name} logging") - except rclpy.exceptions.ParameterNotDeclaredException: - self.get_logger().info( - f"{uri}: No logblock of {topic_name} has been found ") - response.success = False - return response - - response.success = True - return response - - def _add_logging(self, request, response, uri="all"): - """ - Service callback to add logging blocks of the crazyflie - """ - topic_name = request.topic_name - frequency = request.frequency - variables = request.vars - if topic_name in self.default_log_type.keys(): - try: - self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) - self.swarm._cfs[uri].logging[topic_name + "_publisher"] = self.create_publisher( - self.default_log_type[topic_name], self.cf_dict[uri] + "/" + topic_name, 10) - self.swarm._cfs[uri].logging[topic_name + "_log_config"].period_in_ms = 1000 / frequency - self.swarm._cfs[uri].logging[topic_name + "_log_config"].start() - self.get_logger().info(f"{uri}: Add {topic_name} logging") - except rclpy.exceptions.ParameterAlreadyDeclaredException: - self.get_logger().info( - f"{uri}: The content the logging of {topic_name} has already started ") - response.success = False - return response - else: - try: - self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) - self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".vars.", variables) - lg_custom = LogConfig( - name=topic_name, period_in_ms=1000 / frequency) - for log_name in variables: - lg_custom.add_variable(log_name) - self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = self.create_publisher( - LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10) - - self.swarm._cfs[uri].cf.log.add_config(lg_custom) - - lg_custom.data_received_cb.add_callback( - partial(self._log_custom_data_callback, uri=uri)) - lg_custom.error_cb.add_callback(self._log_error_callback) - lg_custom.start() - - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name] = {} - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"] = lg_custom - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency - - self.get_logger().info(f"{uri}: Add {topic_name} logging") - except KeyError as e: - self.get_logger().info( - f"{uri}: Failed to add {topic_name} logging") - self.get_logger().info(str(e) + "is not in TOC") - self.undeclare_parameter(self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") - self.undeclare_parameter(self.cf_dict[uri] + ".logs." + topic_name + ".vars.") - response.success = False - return response - except rclpy.exceptions.ParameterAlreadyDeclaredException: - self.get_logger().info( - f"{uri}: The content or part of the logging of {topic_name} has already started ") - response.success = False - return response - - response.success = True - return response - - -def main(args=None): - - cflib.crtp.init_drivers() - rclpy.init(args=args) - crazyflie_server = CrazyflieServer() - - rclpy.spin(crazyflie_server) - - crazyflie_server.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index af166f74f..8de50c6f4 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -461,26 +461,29 @@ def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory): req.piece_offset = pieceOffset req.pieces = pieces self.uploadTrajectoryService.call_async(req) - + def uploadBezierTrajectory(self, trajectoryId, pieceOffset, trajectory): - """_summary_ + """Uploads a piecewise Bézier trajectory for later execution. + + See bezier_trajectory.py for more information about piecewise Bézier + trajectories. Args: - trajectoryId (_type_): _description_ - pieceOffset (_type_): _description_ - trajectory (_type_): _description_ + trajectoryId (int): ID number of this trajectory. + pieceOffset (int): + trajectory (:obj:`crazyflie_py.bezier_trajectory.BezierTrajectory`): Trajectory object. """ - pieces: list[TrajectoryBezierPiece] = [] - - for traj in trajectory: + pieces = [] + + for curve in trajectory.curve_list: piece = TrajectoryBezierPiece() - piece.duration = rclpy.duration.Duration(seconds=traj.duration).to_msg() - piece.bezier_control_pts_x = traj.x.tolist() - piece.bezier_control_pts_y = traj.y.tolist() - piece.bezier_control_pts_z = traj.z.tolist() - piece.bezier_control_pts_yaw = traj.yaw.tolist() - pieces.append(piece) - + piece.duration = rclpy.duration.Duration(seconds=curve.duration).to_msg() + piece.bezier_control_pts_x = curve.ctrl_pts_x.tolist() + piece.bezier_control_pts_y = curve.ctrl_pts_y.tolist() + piece.bezier_control_pts_z = curve.ctrl_pts_z.tolist() + piece.bezier_control_pts_yaw = curve.ctrl_pts_yaw.tolist() + pieces.append(piece) + req = UploadBezierTrajectory.Request() req.trajectory_id = trajectoryId req.piece_offset = pieceOffset From 7769056619928c247df20896d639e4195bb74893 Mon Sep 17 00:00:00 2001 From: qrthom Date: Tue, 9 Jul 2024 15:12:02 -0400 Subject: [PATCH 08/10] Updated bezier --- crazyflie/scripts/crazyflie_server.py | 50 +++++++------------ .../crazyflie_examples/figure8_bezier.py | 11 +++- crazyflie_examples/setup.cfg | 4 +- .../crazyflie_py/bezier_trajectory.py | 13 +++-- crazyflie_py/crazyflie_py/crazyflie.py | 3 +- 5 files changed, 38 insertions(+), 43 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 0da64a24a..cb8b30436 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -1011,38 +1011,24 @@ def _upload_bezier_trajectory_callback(self, request, response, uri="all"): % (id, offset, num_pieces) ) - # create the trajectories - # example from cflib - # TODO: test if yaw is 0 is the same as empty - a = 0.9 - b = 0.5 - c = 0.5 - trajectory = [ - CompressedStart(0.0, 0.0, 0.0, 0.0), - CompressedSegment(2.0, [0.0, 1.0, 1.0], [0.0, a, 0.0], [], []), - CompressedSegment(2.0, [1.0, b, 0.0], [-a, -c, 0.0], [], []), - CompressedSegment(2.0, [-b, -1.0, -1.0], [c, a, 0.0], [], []), - CompressedSegment(2.0, [-1.0, 0.0, 0.0], [-a, 0.0, 0.0], [], []), - ] - - # trajectory = [] - # start_x = request.pieces[0].bezier_control_pts_x[0] - # start_y = request.pieces[0].bezier_control_pts_y[0] - # start_z = request.pieces[0].bezier_control_pts_z[0] - # start_yaw = 0.0 - # trajectory.append(CompressedStart(start_x, start_y, start_z, start_yaw)) - # for i in range(num_pieces): - # piece = request.pieces[i] - # duration = float(piece.duration.sec) + float(piece.duration.nanosec) / 1e9 - # p = CompressedSegment( - # duration, - # piece.bezier_control_pts_x[1:], - # piece.bezier_control_pts_y[1:], - # piece.bezier_control_pts_z[1:], - # piece.bezier_control_pts_yaw[1:], - # ) - # trajectory.append(p) - # total_duration += duration + trajectory = [] + start_x = request.pieces[0].bezier_control_pts_x[0] + start_y = request.pieces[0].bezier_control_pts_y[0] + start_z = request.pieces[0].bezier_control_pts_z[0] + start_yaw = 0.0 + trajectory.append(CompressedStart(start_x, start_y, start_z, start_yaw)) + for i in range(num_pieces): + piece = request.pieces[i] + duration = float(piece.duration.sec) + float(piece.duration.nanosec) / 1e9 + p = CompressedSegment( + duration, + piece.bezier_control_pts_x[1:], + piece.bezier_control_pts_y[1:], + piece.bezier_control_pts_z[1:], + piece.bezier_control_pts_yaw[1:], + ) + trajectory.append(p) + total_duration += duration if uri == "all": upload_success_all = True diff --git a/crazyflie_examples/crazyflie_examples/figure8_bezier.py b/crazyflie_examples/crazyflie_examples/figure8_bezier.py index d30b9b05e..1a4f1af47 100644 --- a/crazyflie_examples/crazyflie_examples/figure8_bezier.py +++ b/crazyflie_examples/crazyflie_examples/figure8_bezier.py @@ -4,15 +4,22 @@ from pathlib import Path from crazyflie_py import * +from crazyflie_py.bezier_trajectory import BezierTrajectory def main(): + + print( + "WARNING: running a script involving Bezier curves. Currently only cflib backend is supported. Please run the server with 'ros2 launch crazyflie launch.py backend:=cflib'." + ) + swarm = Crazyswarm() timeHelper = swarm.timeHelper allcfs = swarm.allcfs - traj = BezierTrajectory() - traj.from_json(Path(__file__).parent / "data/figure8_bezier.json") + traj = BezierTrajectory.from_json( + Path(__file__).parent / "data/figure8_bezier.json" + ) TRIALS = 1 TIMESCALE = 1.0 diff --git a/crazyflie_examples/setup.cfg b/crazyflie_examples/setup.cfg index 69990ea8d..b8fcfe8ca 100644 --- a/crazyflie_examples/setup.cfg +++ b/crazyflie_examples/setup.cfg @@ -3,9 +3,9 @@ console_scripts = hello_world = crazyflie_examples.hello_world:main nice_hover = crazyflie_examples.nice_hover:main figure8 = crazyflie_examples.figure8:main + figure8_bezier = crazyflie_examples.figure8_bezier:main multi_trajectory = crazyflie_examples.multi_trajectory:main cmd_full_state = crazyflie_examples.cmd_full_state:main set_param = crazyflie_examples.set_param:main hello_world4 = crazyflie_examples.hello_world4:main - drone_drag = crazyflie_examples.drones_drag.drones_in_drag.Strut.run:main - bezier_example = crazyflie_examples.bezier_example:main + drone_drag = crazyflie_examples.drones_drag.drones_in_drag.Strut.run:main \ No newline at end of file diff --git a/crazyflie_py/crazyflie_py/bezier_trajectory.py b/crazyflie_py/crazyflie_py/bezier_trajectory.py index c3a797ecd..1d8fcb33b 100644 --- a/crazyflie_py/crazyflie_py/bezier_trajectory.py +++ b/crazyflie_py/crazyflie_py/bezier_trajectory.py @@ -15,7 +15,7 @@ def __init__( self, nodes, degree, - ctrl_pts_yaw: np.ndarray = [], + ctrl_pts_yaw: np.ndarray = None, duration=None, *args, **kwargs, @@ -27,7 +27,10 @@ def __init__( self.ctrl_pts_y = nodes[1] if nodes.shape[0] == 3: self.ctrl_pts_z = nodes[2] - self.ctrl_pts_yaw = ctrl_pts_yaw + if ctrl_pts_yaw is None: + self.ctrl_pts_yaw = np.zeros_like(self.ctrl_pts_x) + else: + self.ctrl_pts_yaw = ctrl_pts_yaw @classmethod def from_nodes(cls, nodes: np.ndarray, duration: float = None) -> "BezierCurve": @@ -314,7 +317,7 @@ def visualize( control_pts_list = [control_pts1, control_pts2, control_pts3, control_pts4] bezier_traj = BezierTrajectory.from_control_points(control_pts_list) # Alternatively, load from json - # bezier_traj = BezierTrajectory.from_json( - # "crazyflie_examples/crazyflie_examples/data/figure8_bezier.json" - # ) + bezier_traj = BezierTrajectory.from_json( + "crazyflie_examples/crazyflie_examples/data/figure8_bezier.json" + ) bezier_traj.visualize(derivatives=[0], show_plot=True) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 8de50c6f4..1db1f23a1 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -473,7 +473,7 @@ def uploadBezierTrajectory(self, trajectoryId, pieceOffset, trajectory): pieceOffset (int): trajectory (:obj:`crazyflie_py.bezier_trajectory.BezierTrajectory`): Trajectory object. """ - pieces = [] + pieces = [] for curve in trajectory.curve_list: piece = TrajectoryBezierPiece() @@ -817,7 +817,6 @@ def __init__(self, log_poses=False): ) self.cmdFullStateMsg = FullState() self.cmdFullStateMsg.header.frame_id = "/world" - cfnames = [] for srv_name, srv_types in self.get_service_names_and_types(): # print("cfname: ", srv_name[1:-17]) From a2e9f7dd23a3068af486ea1fb60962ab9eea9e28 Mon Sep 17 00:00:00 2001 From: Yutong Wang Date: Tue, 23 Jul 2024 14:13:10 -0400 Subject: [PATCH 09/10] bugfix: multiple trajectory upload for polynomial and bezier --- crazyflie/config/motion_capture.yaml | 2 +- crazyflie/scripts/crazyflie_server.py | 8 ++++---- crazyflie_examples/crazyflie_examples/figure8_bezier.py | 3 +++ crazyflie_py/crazyflie_py/crazyflie.py | 4 ++-- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/crazyflie/config/motion_capture.yaml b/crazyflie/config/motion_capture.yaml index 0048beb2b..1171c3573 100644 --- a/crazyflie/config/motion_capture.yaml +++ b/crazyflie/config/motion_capture.yaml @@ -1,7 +1,7 @@ /motion_capture_tracking: ros__parameters: type: "vicon" - hostname: "169.254.217.218" + hostname: "169.254.122.233" # mode: "libRigidBodyTracker" # one of motionCapture,libRigidBodyTracker # mode: "libRigidBodyTracker" mode: "motionCapture" diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 3c648d356..7474c945c 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -1001,7 +1001,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): for link_uri in self.uris: trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( MemoryElement.TYPE_TRAJ - )[id] + )[0] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: @@ -1017,7 +1017,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): else: trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( MemoryElement.TYPE_TRAJ - )[id] + )[0] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: @@ -1065,7 +1065,7 @@ def _upload_bezier_trajectory_callback(self, request, response, uri="all"): for link_uri in self.uris: trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( MemoryElement.TYPE_TRAJ - )[id] + )[0] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: @@ -1086,7 +1086,7 @@ def _upload_bezier_trajectory_callback(self, request, response, uri="all"): else: trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( MemoryElement.TYPE_TRAJ - )[id] + )[0] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: diff --git a/crazyflie_examples/crazyflie_examples/figure8_bezier.py b/crazyflie_examples/crazyflie_examples/figure8_bezier.py index 1a4f1af47..d5cd9550b 100644 --- a/crazyflie_examples/crazyflie_examples/figure8_bezier.py +++ b/crazyflie_examples/crazyflie_examples/figure8_bezier.py @@ -26,6 +26,7 @@ def main(): for i in range(TRIALS): for cf in allcfs.crazyflies: cf.uploadBezierTrajectory(0, 0, trajectory=traj) + cf.uploadBezierTrajectory(1, 0, trajectory=traj) # upload multiple trajectory is possible allcfs.takeoff(targetHeight=1.0, duration=2.0) timeHelper.sleep(2.5) @@ -36,6 +37,8 @@ def main(): allcfs.startTrajectory(0, timescale=TIMESCALE) timeHelper.sleep(traj.total_time * TIMESCALE + 2.0) + # allcfs.startTrajectory(1, timescale=TIMESCALE) + # timeHelper.sleep(traj.total_time * TIMESCALE + 2.0) timeHelper.sleep(4.5) allcfs.land(targetHeight=0.0, duration=2.0) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 10bbce1df..fc6dbb545 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -19,9 +19,9 @@ from collections import defaultdict import copy -from crazyflie_interfaces.msg import FullState, Position, Status, TrajectoryPolynomialPiece +from crazyflie_interfaces.msg import FullState, Position, Status, TrajectoryPolynomialPiece, TrajectoryBezierPiece from crazyflie_interfaces.srv import Arm, GoTo, Land, \ - NotifySetpointsStop, StartTrajectory, Takeoff, UploadTrajectory + NotifySetpointsStop, StartTrajectory, Takeoff, UploadTrajectory, UploadBezierTrajectory from geometry_msgs.msg import Point import numpy as np from rcl_interfaces.msg import Parameter, ParameterType, ParameterValue From a2a4451851929dc4804d59f79a46ebe9469ee58c Mon Sep 17 00:00:00 2001 From: Yutong Wang Date: Tue, 23 Jul 2024 15:13:52 -0400 Subject: [PATCH 10/10] Redo some formatting --- crazyflie/scripts/crazyflie_server.py | 616 ++++++++++---------------- 1 file changed, 237 insertions(+), 379 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 7474c945c..d449a554b 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -76,49 +76,30 @@ def __init__(self): self.uris = [] # for logging, assign a all -> all mapping - self.cf_dict = {"all": "all"} + 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, - "status": Status, - } - 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, - "status": self._log_status_data_callback, - } + self.default_log_type = {"pose": PoseStamped, + "scan": LaserScan, + "odom": Odometry, + "status": Status} + 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, + "status": self._log_status_data_callback} self.world_tf_name = "world" try: @@ -135,9 +116,8 @@ 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) @@ -157,7 +137,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 ) @@ -170,21 +151,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 @@ -196,35 +171,26 @@ def __init__(self): 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 - ) + 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 @@ -238,26 +204,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: @@ -265,10 +222,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: @@ -277,23 +232,22 @@ def __init__(self): name = self.cf_dict[uri] - pub = self.create_publisher( - String, - name + "/robot_description", - rclpy.qos.QoSProfile( - depth=1, durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL - ), - ) + pub = self.create_publisher(String, name + '/robot_description', + rclpy.qos.QoSProfile( + depth=1, + durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL)) msg = String() - msg.data = self._ros_parameters["robot_description"].replace("$NAME", name) + msg.data = self._ros_parameters['robot_description'].replace("$NAME", name) pub.publish(msg) self.create_service( - Empty, name + "/emergency", partial(self._emergency_callback, uri=uri) + Empty, name + + "/emergency", partial(self._emergency_callback, uri=uri) ) self.create_service( - Takeoff, name + "/takeoff", partial(self._takeoff_callback, uri=uri) + Takeoff, name + + "/takeoff", partial(self._takeoff_callback, uri=uri) ) self.create_service( Land, name + "/land", partial(self._land_callback, uri=uri) @@ -302,14 +256,14 @@ def __init__(self): GoTo, name + "/go_to", partial(self._go_to_callback, uri=uri) ) self.create_service( - StartTrajectory, - name + "/start_trajectory", - partial(self._start_trajectory_callback, uri=uri), + StartTrajectory, name + + "/start_trajectory", partial( + self._start_trajectory_callback, uri=uri) ) self.create_service( - UploadTrajectory, - name + "/upload_trajectory", - partial(self._upload_trajectory_callback, uri=uri), + UploadTrajectory, name + + "/upload_trajectory", partial( + self._upload_trajectory_callback, uri=uri) ) self.create_service( UploadBezierTrajectory, @@ -322,33 +276,27 @@ def __init__(self): partial(self._notify_setpoints_stop_callback, uri=uri), ) self.create_subscription( - Twist, - name + "/cmd_vel_legacy", - partial(self._cmd_vel_legacy_changed, uri=uri), - 10, + Twist, name + + "/cmd_vel_legacy", partial(self._cmd_vel_legacy_changed, + uri=uri), 10 ) self.create_subscription( - Hover, - name + "/cmd_hover", - partial(self._cmd_hover_changed, uri=uri), - 10, + Hover, name + + "/cmd_hover", partial(self._cmd_hover_changed, uri=uri), 10 ) self.create_subscription( - FullState, - name + "/cmd_full_state", - partial(self._cmd_full_state_changed, uri=uri), - 10, + FullState, name + + "/cmd_full_state", partial(self._cmd_full_state_changed, uri=uri), 10 ) - qos_profile = QoSProfile( - reliability=QoSReliabilityPolicy.BEST_EFFORT, + qos_profile = QoSProfile(reliability =QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=1, - deadline=Duration(seconds=0, nanoseconds=1e9 / 100.0), - ) + deadline = Duration(seconds=0, nanoseconds=1e9/100.0)) self.create_subscription( - NamedPoseArray, "/poses", self._poses_changed, qos_profile + NamedPoseArray, "/poses", + self._poses_changed, qos_profile ) # Create services for the entire swarm and each individual crazyflie @@ -356,15 +304,12 @@ def __init__(self): self.create_service(Land, "all/land", self._land_callback) self.create_service(GoTo, "all/go_to", self._go_to_callback) self.create_service( - StartTrajectory, "all/start_trajectory", self._start_trajectory_callback - ) - + 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) - 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 """ @@ -374,43 +319,40 @@ 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") else: lg.add_variable(logvar) - self.swarm._cfs[link_uri].logging[prefix + "_logging_enabled"] = logging_enabled - self.swarm._cfs[link_uri].logging[prefix + "_logging_freq"] = logging_freq + self.swarm._cfs[link_uri].logging[prefix + + "_logging_enabled"] = logging_enabled + 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" @@ -421,8 +363,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, {}) @@ -466,62 +408,38 @@ def _init_logging(self): # Start logging for predefined logging for default_log_name in self.default_log_type: prefix = default_log_name - if ( - cf_handle.logging[prefix + "_logging_enabled"] - and cf_handle.logging["enabled"] - ): + if cf_handle.logging[prefix + "_logging_enabled"] and cf_handle.logging["enabled"]: callback_fnc = self.default_log_fnc[prefix] self._init_default_logging(prefix, link_uri, callback_fnc) # Start logging for costum logging blocks cf_handle.l_toc = cf.log.toc.toc - if ( - len(cf_handle.logging["custom_log_groups"]) != 0 - and cf_handle.logging["enabled"] - ): - - for log_group_name, log_group_dict in cf_handle.logging[ - "custom_log_groups" - ].items(): - self.swarm._cfs[link_uri].logging["custom_log_publisher"][ - log_group_name - ] = self.create_publisher( - LogDataGeneric, - self.cf_dict[link_uri] + "/" + log_group_name, - 10, - ) - lg_custom = log_group_dict["log_config"] + if len(cf_handle.logging["custom_log_groups"]) != 0 and cf_handle.logging["enabled"]: + + for log_group_name, log_group_dict in cf_handle.logging["custom_log_groups"].items(): + self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = self.create_publisher( + LogDataGeneric, self.cf_dict[link_uri] + "/" + log_group_name, 10) + lg_custom = log_group_dict['log_config'] try: cf.log.add_config(lg_custom) lg_custom.data_received_cb.add_callback( - partial(self._log_custom_data_callback, uri=link_uri) - ) - lg_custom.error_cb.add_callback(self._log_error_callback) + partial(self._log_custom_data_callback, uri=link_uri)) + lg_custom.error_cb.add_callback( + self._log_error_callback) lg_custom.start() except KeyError as e: - self.get_logger().info( - f"[{self.cf_dict[link_uri]}] Could not start log configuration," - "{} not found in TOC".format(str(e)) - ) + self.get_logger().info(f'[{self.cf_dict[link_uri]}] Could not start log configuration,' + '{} not found in TOC'.format(str(e))) except AttributeError: self.get_logger().info( - f"[{self.cf_dict[link_uri]}] Could not add log config, bad configuration." - ) + f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.') - self.get_logger().info( - f"[{self.cf_dict[link_uri]}] setup custom logging" - ) + self.get_logger().info(f"[{self.cf_dict[link_uri]}] setup custom logging") self.create_service( - RemoveLogging, - self.cf_dict[link_uri] + "/remove_logging", - partial(self._remove_logging, uri=link_uri), - ) + RemoveLogging, self.cf_dict[link_uri] + "/remove_logging", partial(self._remove_logging, uri=link_uri)) self.create_service( - AddLogging, - self.cf_dict[link_uri] + "/add_logging", - partial(self._add_logging, uri=link_uri), - ) + AddLogging, self.cf_dict[link_uri] + "/add_logging", partial(self._add_logging, uri=link_uri)) self.get_logger().info("All Crazyflies logging are initialized.") @@ -534,37 +452,33 @@ def _init_default_logging(self, prefix, link_uri, callback_fnc): lg = cf_handle.logging[prefix + "_log_config"] try: cf.log.add_config(lg) - lg.data_received_cb.add_callback(partial(callback_fnc, uri=link_uri)) + lg.data_received_cb.add_callback( + partial(callback_fnc, uri=link_uri)) lg.error_cb.add_callback(self._log_error_callback) lg.start() frequency = cf_handle.logging[prefix + "_logging_freq"] self.declare_parameter( - self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency - ) + self.cf_dict[link_uri] + ".logs." + prefix + ".frequency.", frequency) self.get_logger().info( - f"[{self.cf_dict[link_uri]}] setup logging for {prefix} at freq {frequency}" - ) + f"[{self.cf_dict[link_uri]}] setup logging for {prefix} at freq {frequency}") except KeyError as e: - self.get_logger().error( - f"[{self.cf_dict[link_uri]}] Could not start log configuration," - "{} not found in TOC".format(str(e)) - ) + self.get_logger().error(f'[{self.cf_dict[link_uri]}] Could not start log configuration,' + '{} not found in TOC'.format(str(e))) except AttributeError: self.get_logger().error( - f"[{self.cf_dict[link_uri]}] Could not add log config, bad configuration." - ) + f'[{self.cf_dict[link_uri]}] Could not add log config, bad configuration.') 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] max_range = 3.49 - front_range = float(data.get("range.front")) / 1000.0 - left_range = float(data.get("range.left")) / 1000.0 - back_range = float(data.get("range.back")) / 1000.0 - right_range = float(data.get("range.right")) / 1000.0 + front_range = float(data.get('range.front'))/1000.0 + left_range = float(data.get('range.left'))/1000.0 + back_range = float(data.get('range.back'))/1000.0 + right_range = float(data.get('range.right'))/1000.0 if front_range > max_range: front_range = float("inf") if left_range > max_range: @@ -583,23 +497,23 @@ def _log_scan_data_callback(self, timestamp, data, logconf, uri): msg.ranges = self.ranges msg.angle_min = -0.5 * 2 * pi msg.angle_max = 0.25 * 2 * pi - msg.angle_increment = 1.0 * pi / 2 + msg.angle_increment = 1.0 * pi/2 self.swarm._cfs[uri].logging["scan_publisher"].publish(msg) 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 """ cf_name = self.cf_dict[uri] - x = data.get("stateEstimate.x") - y = data.get("stateEstimate.y") - z = data.get("stateEstimate.z") - roll = radians(data.get("stabilizer.roll")) - pitch = radians(-1.0 * data.get("stabilizer.pitch")) - yaw = radians(data.get("stabilizer.yaw")) + x = data.get('stateEstimate.x') + y = data.get('stateEstimate.y') + z = data.get('stateEstimate.z') + roll = radians(data.get('stabilizer.roll')) + pitch = radians(-1.0 * data.get('stabilizer.pitch')) + yaw = radians(data.get('stabilizer.yaw')) q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) msg = PoseStamped() @@ -629,23 +543,23 @@ 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] - x = data.get("stateEstimate.x") - y = data.get("stateEstimate.y") - z = data.get("stateEstimate.z") - yaw = radians(data.get("stabilizer.yaw")) - roll = radians(data.get("stabilizer.roll")) - pitch = radians(data.get("stabilizer.pitch")) - vx = data.get("kalman.statePX") - vy = data.get("kalman.statePY") - vz = data.get("kalman.statePZ") - yawrate = data.get("gyro.z") - rollrate = data.get("gyro.x") - pitchrate = data.get("gyro.y") + x = data.get('stateEstimate.x') + y = data.get('stateEstimate.y') + z = data.get('stateEstimate.z') + yaw = radians(data.get('stabilizer.yaw')) + roll = radians(data.get('stabilizer.roll')) + pitch = radians(data.get('stabilizer.pitch')) + vx = data.get('kalman.statePX') + vy = data.get('kalman.statePY') + vz = data.get('kalman.statePZ') + yawrate = data.get('gyro.z') + rollrate = data.get('gyro.x') + pitchrate = data.get('gyro.y') q = tf_transformations.quaternion_from_euler(roll, pitch, yaw) msg = Odometry() @@ -670,7 +584,7 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri): t_base = TransformStamped() t_base.header.stamp = self.get_clock().now().to_msg() - t_base.header.frame_id = "odom" + t_base.header.frame_id = 'odom' t_base.child_frame_id = cf_name t_base.transform.translation.x = x t_base.transform.translation.y = y @@ -689,16 +603,16 @@ def _log_status_data_callback(self, timestamp, data, logconf, uri): msg = Status() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = self.world_tf_name - msg.supervisor_info = data.get("supervisor.info") - msg.battery_voltage = data.get("pm.vbatMV") / 1000.0 - msg.pm_state = data.get("pm.state") - msg.rssi = data.get("radio.rssi") + msg.supervisor_info = data.get('supervisor.info') + msg.battery_voltage = data.get('pm.vbatMV') / 1000.0 + msg.pm_state = data.get('pm.state') + msg.rssi = data.get('radio.rssi') self.swarm._cfs[uri].logging["status_publisher"].publish(msg) 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() @@ -707,14 +621,15 @@ def _log_custom_data_callback(self, timestamp, data, logconf, uri): for log_name in data: msg.values.append(data.get(log_name)) - self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish(msg) + self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish( + msg) def _log_error_callback(self, logconf, msg): - print("Error when logging %s: %s" % (logconf.name, msg)) + print('Error when logging %s: %s' % (logconf.name, 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 @@ -731,29 +646,24 @@ 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=cf_log_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 try: - set_param_value = self._ros_parameters["all"][ - "firmware_params" - ][group][param] + set_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] - ]["firmware_params"][group][param] + set_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] - ]["firmware_params"][group][param] + set_param_value = self._ros_parameters["robots"][self.cf_dict[link_uri] + ]["firmware_params"][group][param] except KeyError: pass @@ -767,7 +677,8 @@ def _init_parameters(self): f"[{self.cf_dict[link_uri]}] {name} is set to {set_param_value}" ) self.declare_parameter( - self.cf_dict[link_uri] + ".params." + group + "." + param, + self.cf_dict[link_uri] + + ".params." + group + "." + param, value=set_param_value, descriptor=parameter_descriptor, ) @@ -776,32 +687,21 @@ def _init_parameters(self): # If value is not found in initial parameter set # get crazyflie paramter value and declare that value in ROS 2 parameter - if ( - cf_log_to_ros_param[type_cf_param] - is ParameterType.PARAMETER_INTEGER - ): + 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 - ): + elif cf_log_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, + 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 - ): + 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 - ): + 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( @@ -856,6 +756,7 @@ def _parameters_callback(self, params): if param_split[1] == "logs": return SetParametersResult(successful=True) + return SetParametersResult(successful=False) def _emergency_callback(self, request, response, uri="all"): @@ -869,11 +770,12 @@ 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 """ - duration = float(request.duration.sec) + float(request.duration.nanosec / 1e9) + duration = float(request.duration.sec) + \ + float(request.duration.nanosec / 1e9) self.get_logger().info( f"[{self.cf_dict[uri]}] takeoff(height={request.height} m," + f"duration={duration} s," @@ -893,10 +795,11 @@ 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) + float(request.duration.nanosec / 1e9) + duration = float(request.duration.sec) + \ + float(request.duration.nanosec / 1e9) self.get_logger().info( f"[{self.cf_dict[uri]}] land(height={request.height} m," + f"duration={duration} s," @@ -916,10 +819,11 @@ 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) + float(request.duration.nanosec / 1e9) + duration = float(request.duration.sec) + \ + float(request.duration.nanosec / 1e9) self.get_logger().info( "[%s] go_to(position=%f,%f,%f m, yaw=%f rad, duration=%f s, relative=%d, group_mask=%d)" @@ -975,15 +879,12 @@ def _upload_trajectory_callback(self, request, response, uri="all"): offset = request.piece_offset length = len(request.pieces) total_duration = 0 - self.get_logger().info( - "[%s] upload_trajectory(id=%d,offset=%d, lenght=%d)" - % ( - self.cf_dict[uri], - id, - offset, - length, - ) - ) + self.get_logger().info("[%s] upload_trajectory(id=%d,offset=%d, length=%d)" % ( + self.cf_dict[uri], + id, + offset, + length, + )) trajectory = [] for i in range(length): @@ -992,7 +893,8 @@ def _upload_trajectory_callback(self, request, response, uri="all"): py = Poly4D.Poly(piece.poly_y) pz = Poly4D.Poly(piece.poly_z) pyaw = Poly4D.Poly(piece.poly_yaw) - duration = float(piece.duration.sec) + float(piece.duration.nanosec) / 1e9 + duration = float(piece.duration.sec) + \ + float(piece.duration.nanosec)/1e9 trajectory.append(Poly4D(duration, px, py, pz, pyaw)) total_duration = total_duration + duration @@ -1000,8 +902,7 @@ def _upload_trajectory_callback(self, request, response, uri="all"): upload_success_all = True for link_uri in self.uris: trajectory_mem = self.swarm._cfs[link_uri].cf.mem.get_mems( - MemoryElement.TYPE_TRAJ - )[0] + MemoryElement.TYPE_TRAJ)[0] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: @@ -1009,15 +910,13 @@ def _upload_trajectory_callback(self, request, response, uri="all"): upload_success_all = False else: self.swarm._cfs[link_uri].cf.high_level_commander.define_trajectory( - id, offset, len(trajectory) - ) + id, offset, len(trajectory)) if upload_success_all is False: response.success = False return response else: trajectory_mem = self.swarm._cfs[uri].cf.mem.get_mems( - MemoryElement.TYPE_TRAJ - )[0] + MemoryElement.TYPE_TRAJ)[0] trajectory_mem.trajectory = trajectory upload_result = trajectory_mem.write_data_sync() if not upload_result: @@ -1112,19 +1011,21 @@ def _start_trajectory_callback(self, request, response, uri="all"): rev = request.reversed gm = request.group_mask - self.get_logger().info( - "[%s] start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" - % (self.cf_dict[uri], id, ts, rel, rev, gm) - ) + self.get_logger().info("[%s] start_trajectory(id=%d,timescale=%f,relative=%d, reversed=%d, group_mask=%d)" % ( + self.cf_dict[uri], + id, + ts, + rel, + rev, + gm + )) if uri == "all": for link_uri in self.uris: self.swarm._cfs[link_uri].cf.high_level_commander.start_trajectory( - id, ts, rel, rev, gm - ) + id, ts, rel, rev, gm) else: self.swarm._cfs[uri].cf.high_level_commander.start_trajectory( - id, ts, rel, rev, gm - ) + id, ts, rel, rev, gm) return response @@ -1132,7 +1033,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 @@ -1147,11 +1048,11 @@ def _poses_changed(self, msg): uri = self.uri_dict[name] # self.get_logger().info(f"{uri}: send extpos {x}, {y}, {z} to {name}") if isnan(quat.x): - self.swarm._cfs[uri].cf.extpos.send_extpos(x, y, z) + self.swarm._cfs[uri].cf.extpos.send_extpos( + x, y, z) else: self.swarm._cfs[uri].cf.extpos.send_extpose( - x, y, z, quat.x, quat.y, quat.z, quat.w - ) + x, y, z, quat.x, quat.y, quat.z, quat.w) def _cmd_vel_legacy_changed(self, msg, uri=""): """ @@ -1162,7 +1063,8 @@ def _cmd_vel_legacy_changed(self, msg, uri=""): pitch = -msg.linear.x yawrate = msg.angular.z thrust = int(min(max(msg.linear.z, 0, 0), 60000)) - self.swarm._cfs[uri].cf.commander.send_setpoint(roll, pitch, yawrate, thrust) + self.swarm._cfs[uri].cf.commander.send_setpoint( + roll, pitch, yawrate, thrust) def _cmd_hover_changed(self, msg, uri=""): """ @@ -1172,8 +1074,9 @@ def _cmd_hover_changed(self, msg, uri=""): vx = msg.vx vy = msg.vy z = msg.z_distance - yawrate = -1.0 * degrees(msg.yaw_rate) - self.swarm._cfs[uri].cf.commander.send_hover_setpoint(vx, vy, yawrate, z) + yawrate = -1.0*degrees(msg.yaw_rate) + self.swarm._cfs[uri].cf.commander.send_hover_setpoint( + vx, vy, yawrate, z) def _cmd_full_state_changed(self, msg, uri=""): """ @@ -1182,19 +1085,12 @@ def _cmd_full_state_changed(self, msg, uri=""): pos = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z] vel = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z] acc = [msg.acc.x, msg.acc.y, msg.acc.z] - q = [ - msg.pose.orientation.x, - msg.pose.orientation.y, - msg.pose.orientation.z, - msg.pose.orientation.w, - ] + q = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w] roll_rate = msg.twist.angular.x - pitch_rate = msg.twist.angular.y + 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 - ) - + 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 @@ -1203,39 +1099,27 @@ def _remove_logging(self, request, response, uri="all"): if topic_name in self.default_log_type.keys(): try: self.undeclare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency." - ) + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") self.swarm._cfs[uri].logging[topic_name + "_log_config"].stop() self.destroy_publisher( - self.swarm._cfs[uri].logging[topic_name + "_publisher"] - ) - self.get_logger().info( - f"[{self.cf_dict[uri]}] Remove {topic_name} logging" - ) + self.swarm._cfs[uri].logging[topic_name + "_publisher"]) + self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging") except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found " - ) + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ") response.success = False return response else: try: - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ - "log_config" - ].stop() - for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][ - topic_name - ]["vars"]: - self.destroy_publisher( - self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] - ) - self.get_logger().info( - f"[{self.cf_dict[uri]}] Remove {topic_name} logging" + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"].stop( ) + for log_name in self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"]: + self.destroy_publisher( + self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name]) + self.get_logger().info(f"[{self.cf_dict[uri]}] Remove {topic_name} logging") except rclpy.exceptions.ParameterNotDeclaredException: self.get_logger().info( - f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found " - ) + f"[{self.cf_dict[uri]}] No logblock of {topic_name} has been found ") response.success = False return response @@ -1252,84 +1136,58 @@ def _add_logging(self, request, response, uri="all"): if topic_name in self.default_log_type.keys(): try: self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency - ) - self.swarm._cfs[uri].logging[topic_name + "_publisher"] = ( - self.create_publisher( - self.default_log_type[topic_name], - self.cf_dict[uri] + "/" + topic_name, - 10, - ) - ) - self.swarm._cfs[uri].logging[ - topic_name + "_log_config" - ].period_in_ms = (1000 / frequency) - self.swarm._cfs[uri].logging[topic_name + "_log_config"].start() - self.get_logger().info( - f"[{self.cf_dict[uri]}] Add {topic_name} logging" - ) + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) + self.swarm._cfs[uri].logging[topic_name + "_publisher"] = self.create_publisher( + self.default_log_type[topic_name], self.cf_dict[uri] + "/" + topic_name, 10) + self.swarm._cfs[uri].logging[topic_name + + "_log_config"].period_in_ms = 1000 / frequency + self.swarm._cfs[uri].logging[topic_name + + "_log_config"].start() + self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging") except rclpy.exceptions.ParameterAlreadyDeclaredException: self.get_logger().info( - f"[{self.cf_dict[uri]}] The content the logging of {topic_name} has already started " - ) + f"[{self.cf_dict[uri]}] The content the logging of {topic_name} has already started ") response.success = False return response else: try: self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency - ) + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.", frequency) self.declare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".vars.", variables - ) - lg_custom = LogConfig(name=topic_name, period_in_ms=1000 / frequency) + self.cf_dict[uri] + ".logs." + topic_name + ".vars.", variables) + lg_custom = LogConfig( + name=topic_name, period_in_ms=1000 / frequency) for log_name in variables: lg_custom.add_variable(log_name) - self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = ( - self.create_publisher( - LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10 - ) - ) + self.swarm._cfs[uri].logging["custom_log_publisher"][topic_name] = self.create_publisher( + LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10) self.swarm._cfs[uri].cf.log.add_config(lg_custom) lg_custom.data_received_cb.add_callback( - partial(self._log_custom_data_callback, uri=uri) - ) + partial(self._log_custom_data_callback, uri=uri)) lg_custom.error_cb.add_callback(self._log_error_callback) lg_custom.start() self.swarm._cfs[uri].logging["custom_log_groups"][topic_name] = {} - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ - "log_config" - ] = lg_custom - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ - "vars" - ] = variables - self.swarm._cfs[uri].logging["custom_log_groups"][topic_name][ - "frequency" - ] = frequency + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["log_config"] = lg_custom + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["vars"] = variables + self.swarm._cfs[uri].logging["custom_log_groups"][topic_name]["frequency"] = frequency - self.get_logger().info( - f"[{self.cf_dict[uri]}] Add {topic_name} logging" - ) + self.get_logger().info(f"[{self.cf_dict[uri]}] Add {topic_name} logging") except KeyError as e: self.get_logger().error( - f"[{self.cf_dict[uri]}] Failed to add {topic_name} logging" - ) + f"[{self.cf_dict[uri]}] Failed to add {topic_name} logging") self.get_logger().error(str(e) + "is not in TOC") self.undeclare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".frequency." - ) + self.cf_dict[uri] + ".logs." + topic_name + ".frequency.") self.undeclare_parameter( - self.cf_dict[uri] + ".logs." + topic_name + ".vars." - ) + self.cf_dict[uri] + ".logs." + topic_name + ".vars.") response.success = False return response except rclpy.exceptions.ParameterAlreadyDeclaredException: self.get_logger().error( - f"[{self.cf_dict[uri]}] The content or part of the logging of {topic_name} has already started " - ) + f"[{self.cf_dict[uri]}] The content or part of the logging of {topic_name} has already started ") response.success = False return response