Merge pull request #578 from IMRCLab/arm_close_fix

Add arming and fix node closing issue in cflib

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)
@@ -527,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):
         """
@@ -555,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()
@@ -568,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):
         """
@@ -609,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()
@@ -622,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):
         """
@@ -637,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):
         """
@@ -650,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))
@@ -786,13 +819,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)

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     |
 +---------------------+---------+-----------+---------+