From a9153a35dccf32f4e40ebc4e579cb6a71c64d0f0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Oct 2024 16:08:01 +0200 Subject: [PATCH 1/4] add arming request --- crazyflie/scripts/crazyflie_server.py | 29 ++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 249fb9d71..308857f88 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -25,6 +25,7 @@ from crazyflie_interfaces.srv import Takeoff, Land, GoTo, RemoveLogging, AddLogging from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop +from crazyflie_interfaces.srv import Arm from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType from crazyflie_interfaces.msg import Status, Hover, LogDataGeneric, FullState from motion_capture_tracking_interfaces.msg import NamedPoseArray @@ -257,6 +258,10 @@ def __init__(self): Empty, name + "/emergency", partial(self._emergency_callback, uri=uri) ) + self.create_service( + Arm, name + + "/arm", partial(self._arm_callback, uri=uri) + ) self.create_service( Takeoff, name + "/takeoff", partial(self._takeoff_callback, uri=uri) @@ -307,6 +312,7 @@ def __init__(self): ) # Create services for the entire swarm and each individual crazyflie + self.create_service(Arm, "all/arm", self._arm_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) @@ -786,13 +792,34 @@ def _emergency_callback(self, request, response, uri="all"): return response + def _arm_callback(self, request, response, uri="all"): + """ + Service callback to arm or disarm the Crazyflie + """ + + arm_bool = request.arm + + self.get_logger().info( + f"[{self.cf_dict[uri]}] Arm request is {arm_bool} " + ) + if uri == "all": + for link_uri in self.uris: + self.swarm._cfs[link_uri].cf.platform.send_arming_request( + arm_bool + ) + else: + self.swarm._cfs[uri].cf.platform.send_arming_request( + arm_bool + ) + + 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 """ - print("call1 ", uri) duration = float(request.duration.sec) + \ float(request.duration.nanosec / 1e9) From f55f0004d6c3136ddc262ead99a6d52126da11b2 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Oct 2024 16:23:15 +0200 Subject: [PATCH 2/4] if publishing fails, close the log --- crazyflie/scripts/crazyflie_server.py | 41 ++++++++++++++++++++++----- 1 file changed, 34 insertions(+), 7 deletions(-) diff --git a/crazyflie/scripts/crazyflie_server.py b/crazyflie/scripts/crazyflie_server.py index 308857f88..367ecc53e 100755 --- a/crazyflie/scripts/crazyflie_server.py +++ b/crazyflie/scripts/crazyflie_server.py @@ -533,7 +533,11 @@ def _log_scan_data_callback(self, timestamp, data, logconf, uri): 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) + try: + self.swarm._cfs[uri].logging["scan_publisher"].publish(msg) + except: + self.get_logger().info("Could not publish scan message, stopping scan log") + self.swarm._cfs[uri].logging["scan_log_config"].stop() def _log_pose_data_callback(self, timestamp, data, logconf, uri): """ @@ -561,7 +565,11 @@ def _log_pose_data_callback(self, timestamp, data, logconf, uri): 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) + try: + self.swarm._cfs[uri].logging["pose_publisher"].publish(msg) + except: + self.get_logger().info("Could not publish pose message, stopping pose log") + self.swarm._cfs[uri].logging["pose_log_config"].stop() t_base = TransformStamped() t_base.header.stamp = self.get_clock().now().to_msg() @@ -574,7 +582,10 @@ def _log_pose_data_callback(self, timestamp, data, logconf, uri): 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) + try: + self.tfbr.sendTransform(t_base) + except: + self.get_logger().info("Could not publish pose tf") def _log_odom_data_callback(self, timestamp, data, logconf, uri): """ @@ -615,7 +626,11 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri): msg.twist.twist.angular.y = pitchrate msg.twist.twist.angular.x = rollrate - self.swarm._cfs[uri].logging["odom_publisher"].publish(msg) + try: + self.swarm._cfs[uri].logging["odom_publisher"].publish(msg) + except: + self.get_logger().info("Could not publish odom message, stopping odom log") + self.swarm._cfs[uri].logging["odom_log_config"].stop() t_base = TransformStamped() t_base.header.stamp = self.get_clock().now().to_msg() @@ -628,7 +643,11 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri): 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) + + try: + self.tfbr.sendTransform(t_base) + except: + self.get_logger().info("Could not publish odom tf") def _log_status_data_callback(self, timestamp, data, logconf, uri): """ @@ -643,7 +662,11 @@ def _log_status_data_callback(self, timestamp, data, logconf, uri): msg.pm_state = data.get('pm.state') msg.rssi = data.get('radio.rssi') - self.swarm._cfs[uri].logging["status_publisher"].publish(msg) + try: + self.swarm._cfs[uri].logging["status_publisher"].publish(msg) + except: + self.get_logger().info("Could not publish status message, stopping status log") + self.swarm._cfs[uri].logging["status_log_config"].stop() def _log_custom_data_callback(self, timestamp, data, logconf, uri): """ @@ -656,8 +679,12 @@ 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( + try: + self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish( msg) + except: + self.get_logger().info("Could not publish custom {logconf.name} message, stopping custom log") + self.swarm._cfs[uri].logging["custom_log_groups"][logconf.name]["log_config"].stop() def _log_error_callback(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) From 702206cfe61287dc875613105b5a5414f310c243 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Oct 2024 16:23:22 +0200 Subject: [PATCH 3/4] update arming doc --- docs2/overview.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs2/overview.rst b/docs2/overview.rst index b504d7bfd..2f1cc34d5 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -42,7 +42,7 @@ It also setups several flight command services: - **Takeoff / Land / GoTo**: With a single service command and a given height or coordinate, you can make the connected crazyflies take off, go to a position and land. - **Upload / Start trajectory**: You can upload a predefined trajectory and indicate if the Crazyflies need to start flying it. - **Emergency** : To turn off the motors in case something goes wrong. -- **/all or /cf2** : The services are setup either for all crazyflies to respond to, or each individual crazyflie, depended on the prefix. +- **/all or /cf2** : The services are setup either for all crazyflies to respond to, or each individual crazyflie, depended on the prefix. Simulation ---------- @@ -103,5 +103,5 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | Misc | +---------------------+---------+-----------+---------+ -| - Arming | Yes | No | n/a | +| - Arming | Yes | Yes | n/a | +---------------------+---------+-----------+---------+ \ No newline at end of file From 8770c0025596b8017b1e4c40bbaecc48a23cfccd Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 16 Oct 2024 17:20:23 +0200 Subject: [PATCH 4/4] fix doc --- docs2/overview.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs2/overview.rst b/docs2/overview.rst index 2f1cc34d5..2b1cc902c 100644 --- a/docs2/overview.rst +++ b/docs2/overview.rst @@ -103,5 +103,5 @@ Support functionality with backends +---------------------+---------+-----------+---------+ | Misc | +---------------------+---------+-----------+---------+ -| - Arming | Yes | Yes | n/a | +| - Arming | Yes | Yes | n/a | +---------------------+---------+-----------+---------+ \ No newline at end of file