fix: multicontrol_node

ls2n_drone_command_center/multi_control.py

@@ -37,15 +37,17 @@ class MultiControl(Node):
             type=ParameterType.PARAMETER_STRING_ARRAY,
             description="Array of drone names (namespaces) you want to control",
         )
-        self.declare_parameter("drones_to_control", None, descriptor=drones_to_control_descr)
+        self.declare_parameter(
+            "drones_to_control", [""], descriptor=drones_to_control_descr
+        )
         self.drones = self.get_parameter("drones_to_control").value
-        
-        self.drone_request_client = [] # Request client for each drone
-        self.drone_keep_alive_pub = [] # KeepAlive message publisher for each drone
-        self.status_drone_sub = [] # DroneStatus message subscriber for each drone
-        self.drone_status = [] # Drone status
-        self.drone_start_traj_client = [] # Start trajectory client for each drone
-        self.drone_reset_traj_client = [] # Reset trajectory client for each drone
+
+        self.drone_request_client = []  # Request client for each drone
+        self.drone_keep_alive_pub = []  # KeepAlive message publisher for each drone
+        self.status_drone_sub = []  # DroneStatus message subscriber for each drone
+        self.drone_status = []  # Drone status
+        self.drone_start_traj_client = []  # Start trajectory client for each drone
+        self.drone_reset_traj_client = []  # Reset trajectory client for each drone
         self.status = DroneStatus()
         self.status.status = DroneStatus.IDLE
         self.arming_time_init = None
@@ -56,7 +58,10 @@ class MultiControl(Node):
         self.conditions = [None] * DroneRequest.Request.LAST
         self.conditions[DroneRequest.Request.ARM] = {DroneStatus.IDLE}
         self.conditions[DroneRequest.Request.TAKE_OFF] = {DroneStatus.ARMED}
-        self.conditions[DroneRequest.Request.LAND] = { DroneStatus.FLYING, DroneStatus.TAKE_OFF}
+        self.conditions[DroneRequest.Request.LAND] = {
+            DroneStatus.FLYING,
+            DroneStatus.TAKE_OFF,
+        }
 
         self.actions = [None] * DroneRequest.Request.LAST
         self.actions[DroneRequest.Request.ARM] = self.start_arm
@@ -64,15 +69,17 @@ class MultiControl(Node):
         self.actions[DroneRequest.Request.LAND] = self.start_land
 
         # Create publishers / subscribers and services / clients
-        self.status_pub = self.create_publisher(DroneStatus, "Status", qos_profile_sensor_data)
+        self.status_pub = self.create_publisher(
+            DroneStatus, "Status", qos_profile_sensor_data
+        )
         self.keep_alive_sub = self.create_subscription(
             KeepAlive, "keep_alive", self.keep_alive, qos_profile_sensor_data
         )
 
         self.create_service(DroneRequest, "request", self.handle_request)
-        #self.create_service(Empty, "StartTrajectory", self.start_trajectory)
-        #self.create_service(Empty, "ResetTrajectory", self.start_trajectory) # Combined with StopExperiment service
-        
+        # self.create_service(Empty, "StartTrajectory", self.start_trajectory)
+        # self.create_service(Empty, "ResetTrajectory", self.start_trajectory) # Combined with StopExperiment service
+
         # Create publishers / subscribers and services / clients for each drone
         for index, drone_namespace in enumerate(self.drones):
             self.drone_status.append(DroneStatus.IDLE)
@@ -110,7 +117,9 @@ class MultiControl(Node):
         self.loop_land_timer = self.create_timer(3, self.repeat_land)
         self.loop_land_timer.cancel()
 
-    def handle_request(self, request: DroneRequest.Request, response: DroneRequest.Response):
+    def handle_request(
+        self, request: DroneRequest.Request, response: DroneRequest.Response
+    ):
         """
         This function manages user requests
         :return response:
@@ -156,18 +165,17 @@ class MultiControl(Node):
         Returns:
             Empty: empty response
         """
-        self.get_logger().info('Start trajectory request made')
+        self.get_logger().info("Start trajectory request made")
 
         for i in range(len(self.drones)):
-            if self.drone_status[i] in { DroneStatus.FLYING }:
+            if self.drone_status[i] in {DroneStatus.FLYING}:
                 self.drone_start_traj_client[i].call_async(Empty.Request())
 
         return response
 
     def reset_trajectory(self) -> None:
-        """Function used to send a service call to reset the trajectory for each drone
-        """
-        self.get_logger().info('Reset trajectory request made')
+        """Function used to send a service call to reset the trajectory for each drone"""
+        self.get_logger().info("Reset trajectory request made")
         for i in range(len(self.drones)):
             self.drone_reset_traj_client[i].call_async(Empty.Request())
 
@@ -187,8 +195,10 @@ class MultiControl(Node):
         This function monitors the status of the drones and applies the associated procedure and the
         updates the status of the node.
         """
-        if self.status.status in { DroneStatus.ARMED, DroneStatus.STOPPING } \
-            and all(self.drone_status[i] == DroneStatus.IDLE for i in range(len(self.drone_status))):
+        if self.status.status in {DroneStatus.ARMED, DroneStatus.STOPPING} and all(
+            self.drone_status[i] == DroneStatus.IDLE
+            for i in range(len(self.drone_status))
+        ):
             self.status.status = DroneStatus.IDLE
 
         if self.status.status == DroneStatus.PRE_ARMED:
@@ -200,9 +210,10 @@ class MultiControl(Node):
         if self.status.status == DroneStatus.LANDING:
             self.land_process()
 
-        if self.status.status != DroneStatus.IDLE \
-            and any(self.drone_status[i] == DroneStatus.EMERGENCY_STOP \
-                    for i in range(len(self.drone_status))):
+        if self.status.status != DroneStatus.IDLE and any(
+            self.drone_status[i] == DroneStatus.EMERGENCY_STOP
+            for i in range(len(self.drone_status))
+        ):
             self.status.status = DroneStatus.EMERGENCY_STOP
         self.update_status()
 
@@ -247,7 +258,7 @@ class MultiControl(Node):
         :return response: The response of the service.
         """
 
-        self.get_logger().info('Start ARM request made')
+        self.get_logger().info("Start ARM request made")
 
         response = DroneRequest.Response()
         self.arming_time_init = self.get_clock().now().nanoseconds
@@ -267,7 +278,10 @@ class MultiControl(Node):
         """
         This procedure is executed when the status of the node is PRE_ARMED
         """
-        if all(self.drone_status[i] == DroneStatus.ARMED for i in range(len(self.drone_status))):
+        if all(
+            self.drone_status[i] == DroneStatus.ARMED
+            for i in range(len(self.drone_status))
+        ):
             self.loop_arm_timer.cancel()
             self.status.status = DroneStatus.ARMED
             self.get_logger().info("Arming was successful!")
@@ -276,7 +290,9 @@ class MultiControl(Node):
             self.drone_status[i] in {DroneStatus.IDLE, DroneStatus.PRE_ARMED}
             for i in range(len(self.drone_status))
         ):
-            arming_time = (self.get_clock().now().nanoseconds - self.arming_time_init) / 1e9
+            arming_time = (
+                self.get_clock().now().nanoseconds - self.arming_time_init
+            ) / 1e9
             # if arming_time > 5.0:
             #     if self.status.status != DroneStatus.ARMED:
             #         self.get_logger().warn(
@@ -308,7 +324,7 @@ class MultiControl(Node):
         :return response: The response of the service
         """
 
-        self.get_logger().info('Start TAKE-OFF request made')
+        self.get_logger().info("Start TAKE-OFF request made")
 
         response = DroneRequest.Response()
         self.takeoff_time_init = self.get_clock().now().nanoseconds
@@ -329,15 +345,13 @@ class MultiControl(Node):
             self.status.status = DroneStatus.FLYING
             self.get_logger().info("Take-off was successful!")
 
-        take_off_time = (self.get_clock().now().nanoseconds - self.takeoff_time_init) / 1e9
-        if (
-            any(
-                self.drone_status[i]
-                not in {DroneStatus.TAKE_OFF, DroneStatus.FLYING}
-                for i in range(len(self.drone_status))
-            )
-            and (take_off_time > 4.0)
-        ):
+        take_off_time = (
+            self.get_clock().now().nanoseconds - self.takeoff_time_init
+        ) / 1e9
+        if any(
+            self.drone_status[i] not in {DroneStatus.TAKE_OFF, DroneStatus.FLYING}
+            for i in range(len(self.drone_status))
+        ) and (take_off_time > 4.0):
             self.get_logger().warn("Take-off failure!")
             self.emergency_landing()
 
@@ -346,7 +360,7 @@ class MultiControl(Node):
         :return: response: The response of the service
         """
 
-        self.get_logger().info('Start LAND request made')
+        self.get_logger().info("Start LAND request made")
 
         response = DroneRequest.Response()
         self.land_time_init = self.get_clock().now().nanoseconds
@@ -373,7 +387,7 @@ class MultiControl(Node):
             for i in range(len(self.drone_status))
         ):
             land_time = (self.get_clock().now().nanoseconds - self.land_time_init) / 1e9
-            #if 4 < land_time < 15:
+            # if 4 < land_time < 15:
             #    self.loop_land_timer.reset()
             if land_time > 15:
                 self.loop_land_timer.cancel()
@@ -409,5 +423,5 @@ def main(args=None):
         rclpy.try_shutdown()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()