Merge alpha branch

.github/ISSUE_TEMPLATE/bug_report.md

@@ -0,0 +1,38 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: ''
+labels: bug
+assignees: ''
+
+---
+
+**Describe the bug**
+A clear and concise description of what the bug is.
+
+**To Reproduce**
+Steps to reproduce the behavior:
+1. Go to '...'
+2. Click on '....'
+3. Scroll down to '....'
+4. See error
+
+**Expected behavior**
+A clear and concise description of what you expected to happen.
+
+**Screenshots**
+If applicable, add screenshots to help explain your problem.
+
+**Desktop (please complete the following information):**
+ - OS: [e.g. iOS]
+ - Browser [e.g. chrome, safari]
+ - Version [e.g. 22]
+
+**Smartphone (please complete the following information):**
+ - Device: [e.g. iPhone6]
+ - OS: [e.g. iOS8.1]
+ - Browser [e.g. stock browser, safari]
+ - Version [e.g. 22]
+
+**Additional context**
+Add any other context about the problem here.

.github/ISSUE_TEMPLATE/feature_request.md

@@ -0,0 +1,20 @@
+---
+name: Feature request
+about: Suggest an idea for this project
+title: ''
+labels: enhancement
+assignees: ''
+
+---
+
+**Is your feature request related to a problem? Please describe.**
+A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
+
+**Describe the solution you'd like**
+A clear and concise description of what you want to happen.
+
+**Describe alternatives you've considered**
+A clear and concise description of any alternative solutions or features you've considered.
+
+**Additional context**
+Add any other context or screenshots about the feature request here.

.gitignore

@@ -109,5 +109,15 @@ Drone/test_animation/
 Drone/animation.csv
 Drone/client_logs
 images/
-
+.vscode/
 \.idea/
+
+Drone/_copter_client_old_\.py
+
+Drone/test_cl\.py
+
+Server/testj\.ipynb
+
+Server/tst_client\.py
+
+Server/tst\.py

Drone/FlightLib/FlightLib.py

@@ -22,6 +22,8 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
 arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
 landing = rospy.ServiceProxy('/land', Trigger)
 
+services_list = [navigate, set_position, set_rates, set_mode, get_telemetry, arming,landing]
+
 logger.info("Proxy services inited")
 
 # globals
@@ -41,9 +43,11 @@ FLIP_MIN_Z = 2.0
 
 checklist = []
 
-def arming_wrapper(state=False, interrupter=INTERRUPTER):
+
+def arming_wrapper(state=False, *args, **kwargs):
     arming(state)
 
+
 def interrupt():
     logger.info("Performing function interrupt")
     INTERRUPTER.set()
@@ -63,10 +67,8 @@ def check(check_name):
     def inner(f):
         def wrapper(*args, **kwargs):
             failures = f(*args, **kwargs)
-            print(failures)
             msgs = []
             for failure in failures:
-                #print(failure)
                 msg = "[{}]: Failure: {}".format(check_name, failure)
                 msgs.append(msg)
                 logger.warning(msg)
@@ -87,6 +89,16 @@ def _check_nans(*values):
     return any(math.isnan(x) for x in values)
 
 
+@check("Ros services")
+def check_ros_services():
+    timeout = 5.0
+    for service in services_list:
+        try:
+            service.wait_for_service(timeout=timeout)
+        except (rospy.ServiceException, rospy.ROSException) as e:
+            yield ("ROS service {} is not available!".format(service.name))
+
+
 @check("FCU connection")
 def check_connection():
     telemetry = get_telemetry()
@@ -95,7 +107,7 @@ def check_connection():
 
 
 @check("Linear velocity estimation")
-def check_linear_speeds(speed_limit=0.1):
+def check_linear_speeds(speed_limit=0.15):
     telemetry = get_telemetry(frame_id='body')
 
     if _check_nans(telemetry.vx, telemetry.vy, telemetry.vz):
@@ -367,6 +379,7 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc
     if start_telemetry.z < min_z - TOLERANCE:
         logger.warning("Can't do flip! Flip failed!")
         #print("Can't do flip! Flip failed!")
+        return False
     else:                    
         # Flip!    
         set_rates(thrust=1)  # bump up
@@ -383,3 +396,8 @@ def flip(min_z = FLIP_MIN_Z, frame_id = FRAME_ID): #TODO Flip in different direc
         logger.info('Flip succeeded!')
         #print('Flip succeeded!')
         navto(x=start_telemetry.x, y=start_telemetry.y, z=start_telemetry.z, yaw=start_telemetry.yaw, frame_id=frame_id)   # finish flip
+<<<<<<< HEAD
+=======
+
+        return True
+>>>>>>> alpha

Drone/animation_lib.py

@@ -13,16 +13,20 @@ logger = logging.getLogger(__name__)
 
 interrupt_event = threading.Event()
 
-id = "Empty id"
+anim_id = "Empty id"
+
+# TODO refactor as class
+# TODO separate code for frames transformations (e.g. for gps)
+
 
 def get_id(filepath="animation.csv"):
-    global id
+    global anim_id
     try:
         animation_file = open(filepath)
     except IOError:
         logging.error("File {} can't be opened".format(filepath))
-        id = "No animation"
-        return id
+        anim_id = "No animation"
+        return anim_id
     else:
         with animation_file:
             csv_reader = csv.reader(
@@ -30,20 +34,21 @@ def get_id(filepath="animation.csv"):
             )
             row_0 = csv_reader.next()
             if len(row_0) == 1:
-                id = row_0[0]
-                print("Got animation_id: {}".format(id))
+                anim_id = row_0[0]
+                print("Got animation_id: {}".format(anim_id))
             else:
                 print("No animation id in file")
-    return id
+    return anim_id
+
 
 def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
     imported_frames = []
-    global id
+    global anim_id
     try:
         animation_file = open(filepath)
     except IOError:
         logging.error("File {} can't be opened".format(filepath))
-        id = "No animation"
+        anim_id = "No animation"
     else:
         with animation_file:
             csv_reader = csv.reader(
@@ -51,8 +56,8 @@ def load_animation(filepath="animation.csv", x0=0, y0=0, z0=0):
             )
             row_0 = csv_reader.next()
             if len(row_0) == 1:
-                id = row_0[0]
-                print("Got animation_id: {}".format(id))
+                anim_id = row_0[0]
+                print("Got animation_id: {}".format(anim_id))
             else:
                 print("No animation id in file")
                 frame_number, x, y, z, yaw, red, green, blue = row_0
@@ -87,7 +92,6 @@ def convert_frame(frame):
 
 def execute_frame(point=(), color=(), yaw=float('Nan'), frame_id='aruco_map', use_leds=True,
                   flight_func=FlightLib.navto, flight_kwargs=None, interrupter=interrupt_event):
-
     if flight_kwargs is None:
         flight_kwargs = {}
 
@@ -112,7 +116,7 @@ def execute_animation(frames, frame_delay, frame_id='aruco_map', use_leds=True,
         tasking.wait(next_frame_time, interrupter)
 
 
-def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=True,
+def takeoff(z=1.5, safe_takeoff=True, frame_id='map', timeout=5.0, use_leds=True,
             interrupter=interrupt_event):
     print(interrupter.is_set())
     if use_leds:
@@ -120,9 +124,9 @@ def takeoff(z=1.5, safe_takeoff=True, frame_id = 'map', timeout=5.0, use_leds=Tr
     if interrupter.is_set():
         return
     result = FlightLib.takeoff(z=z, wait=False, timeout_takeoff=timeout, frame_id=frame_id, emergency_land=safe_takeoff,
-                      interrupter=interrupter)
+                               interrupter=interrupter)
     if result == 'not armed':
-        raise Exception('STOP')         # Raise exception to clear task_manager if copter can't arm
+        raise Exception('STOP')  # Raise exception to clear task_manager if copter can't arm
     if interrupter.is_set():
         return
     if use_leds:

Drone/client.py

@@ -15,6 +15,7 @@ current_dir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfra
 parent_dir = os.path.dirname(current_dir)
 sys.path.insert(0, parent_dir) 
 
+#logging.basicConfig(level=logging.INFO)
 logger = logging.getLogger(__name__)
 
 import messaging_lib as messaging
@@ -107,7 +108,7 @@ class Client(object):
             logger.critical("Caught interrupt, exiting!")
             self.selector.close()
 
-    def _reconnect(self, timeout=2, attempt_limit=5):
+    def _reconnect(self, timeout=3.0, attempt_limit=5):
         logger.info("Trying to connect to {}:{} ...".format(self.server_host, self.server_port))
         attempt_count = 0
         while not self.connected:
@@ -133,7 +134,7 @@ class Client(object):
 
             if attempt_count >= attempt_limit:
                 logger.info("Too many attempts. Trying to get new server IP")
-                self.broadcast_bind()
+                self.broadcast_bind(timeout*2, attempt_limit)
                 attempt_count = 0
 
 
@@ -146,30 +147,42 @@ class Client(object):
         self._process_connections()
 
 
-    def broadcast_bind(self):
+    def broadcast_bind(self, timeout=3.0, attempt_limit=5):
         broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
         broadcast_client.bind(("", self.broadcast_port))
+        broadcast_client.settimeout(timeout)
+
+        attempt_count = 0
         try:
-            while True:
-                data, addr = broadcast_client.recvfrom(self.BUFFER_SIZE)
-                message = messaging.MessageManager()
-                message.income_raw = data
-                message.process_message()
-                if message.content:
-                    logger.info("Received broadcast message {} from {}".format(message.content, addr))
-                    if message.content["command"] == "server_ip":
-                        args = message.content["args"]
-                        self.server_port = int(args["port"])
-                        self.server_host = args["host"]
-                        self.write_config(False,
-                                          ConfigOption("SERVER", "port", self.server_port),
-                                          ConfigOption("SERVER", "host", self.server_host))
-                        logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port))
-                        break
+            while attempt_count <= attempt_limit:
+                try:
+                    data, addr = broadcast_client.recvfrom(self.BUFFER_SIZE)
+                except socket.error as error:
+                    logger.warning("Could not receive broadcast due error: {}".format(error))
+                    attempt_count += 1
+                else:
+                    message = messaging.MessageManager()
+                    message.income_raw = data
+                    message.process_message()
+                    if message.content:
+                        logger.info("Received broadcast message {} from {}".format(message.content, addr))
+                        if message.content["command"] == "server_ip":
+                            args = message.content["args"]
+                            self.server_port = int(args["port"])
+                            self.server_host = args["host"]
+                            self.write_config(False,
+                                              ConfigOption("SERVER", "port", self.server_port),
+                                              ConfigOption("SERVER", "host", self.server_host))
+                            logger.info("Binding to new IP: {}:{}".format(self.server_host, self.server_port))
+                            self.on_broadcast_bind()
+                            break
         finally:
             broadcast_client.close()
 
+    def on_broadcast_bind(self):
+        pass
+
     def _process_connections(self):
         while True:
             events = self.selector.select(timeout=1)

Drone/copter_client.py

@@ -12,7 +12,7 @@ import messaging_lib as messaging
 import tasking_lib as tasking
 import animation_lib as animation
 
-#logging.basicConfig(  # TODO all prints as logs
+# logging.basicConfig(  # TODO all prints as logs
 #    level=logging.DEBUG, # INFO
 #    format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
 #    handlers=[
@@ -21,7 +21,8 @@ import animation_lib as animation
 
 logger = logging.getLogger(__name__)
 
-#import ros_logging
+
+# import ros_logging
 
 class CopterClient(client.Client):
     def load_config(self):
@@ -40,26 +41,69 @@ class CopterClient(client.Client):
         self.USE_LEDS = self.config.getboolean('PRIVATE', 'use_leds')
         self.LED_PIN = self.config.getint('PRIVATE', 'led_pin')
 
+    def on_broadcast_bind(self):
+        configure_chrony_ip(self.server_host)
+        restart_service("chrony")
+
     def start(self, task_manager_instance):
         client.logger.info("Init ROS node")
         rospy.init_node('Swarm_client', anonymous=True)
         if self.USE_LEDS:
             LedLib.init_led(self.LED_PIN)
-        
+
         task_manager_instance.start()
 
         super(CopterClient, self).start()
 
 
+def restart_service(name):
+    os.system("systemctl restart {}".format(name))
+
+
+def configure_chrony_ip(ip, path="/etc/chrony/chrony.conf", ip_index=1):
+    try:
+        with open(path, 'r') as f:
+            raw_content = f.read()
+    except IOError as e:
+        print("Reading error {}".format(e))
+        return False
+
+    content = raw_content.split(" ")
+
+    try:
+        current_ip = content[ip_index]
+    except IndexError:
+        print("Something wrong with config")
+        return False
+
+    if "." not in current_ip:
+        print("That's not ip!")
+        return False
+
+    if current_ip != ip:
+        content[ip_index] = ip
+
+        try:
+            with open(path, 'w') as f:
+                f.write(" ".join(content))
+        except IOError:
+            print("Error writing")
+            return False
+
+    return True
+
+
 @messaging.request_callback("selfcheck")
 def _response_selfcheck():
     check = FlightLib.selfcheck()
     return check if check else "OK"
 
+
 @messaging.request_callback("anim_id")
 def _response_animation_id():
     return animation.get_id()
 
+
 @messaging.request_callback("batt_voltage")
 def _response_batt():
     return FlightLib.get_telemetry('body').voltage
@@ -69,41 +113,37 @@ def _response_batt():
 def _response_cell():
     return FlightLib.get_telemetry('body').cell_voltage
 
+
 @messaging.message_callback("test")
 def _command_test(**kwargs):
-    print("test")
+    logger.info("logging info test")
+    print("stdout test")
+
 
 @messaging.message_callback("service_restart")
 def _command_service_restart(**kwargs):
-    os.system("systemctl restart" + kwargs["name"])
+    restart_service(kwargs["name"])
+
+@messaging.message_callback("repair_chrony")
+def _command_chrony_repair():
+    configure_chrony_ip(client.active_client.server_host)
+    restart_service("chrony")
 
 
 @messaging.message_callback("led_test")
-def _command_led_test(*args, **kwargs):
+def _command_led_test(**kwargs):
     LedLib.chase(255, 255, 255)
     time.sleep(2)
     LedLib.off()
 
-@messaging.message_callback("led_fill")
-def _command_emergency_led_fill(**kwargs):
-    r = g = b = 0
-    
-    try:
-        r = kwargs["red"]
-    except KeyError:
-        pass
-    
-    try:
-        g = kwargs["green"]
-    except KeyError:
-        pass
-    try:    
-        b = kwargs["blue"]
-    except KeyError: 
-        pass
-    
-    LedLib.fill(r, g, b)
 
+@messaging.message_callback("led_fill")
+def _command_led_fill(**kwargs):
+    r = kwargs.get("red", 0)
+    g = kwargs.get("green", 0)
+    b = kwargs.get("blue", 0)
+
+    LedLib.fill(r, g, b)
 
 
 @messaging.message_callback("flip")
@@ -139,10 +179,10 @@ def _command_land(**kwargs):
 def _command_disarm(**kwargs):
     task_manager.reset()
     task_manager.add_task(-5, 0, FlightLib.arming_wrapper,
-                        task_kwargs={
-                            "state": False
-                        }
-    )
+                          task_kwargs={
+                              "state": False
+                          }
+                          )
 
 
 @messaging.message_callback("stop")
@@ -151,24 +191,24 @@ def _command_stop(**kwargs):
 
 
 @messaging.message_callback("pause")
-def _command_stop(**kwargs):
+def _command_pause(**kwargs):
     task_manager.pause()
 
 
 @messaging.message_callback("resume")
-def _command_stop(**kwargs):
-    task_manager.resume()
+def _command_resume(**kwargs):
+    task_manager.resume(time_to_start_next_task=kwargs.get("time", 0))
 
 
 @messaging.message_callback("start")
 def _play_animation(**kwargs):
     start_time = float(kwargs["time"])  # TODO
 
-    if (animation.get_id() == 'No animation'):
+    if animation.get_id() == 'No animation':
         print("Can't start animation without animation file!")
         return
 
-    print("Start time = {}, wait for {} seconds".format(start_time, time.time()-start_time))
+    print("Start time = {}, wait for {} seconds".format(start_time, time.time() - start_time))
 
     frames = animation.load_animation(os.path.abspath("animation.csv"),
                                       x0=client.active_client.X0 + client.active_client.X0_COMMON,
@@ -176,59 +216,58 @@ def _play_animation(**kwargs):
                                       )
 
     task_manager.add_task(start_time, 0, animation.takeoff,
-                        task_kwargs={
-                            "z": client.active_client.TAKEOFF_HEIGHT,
-                            "timeout": client.active_client.TAKEOFF_TIME,
-                            "safe_takeoff": client.active_client.SAFE_TAKEOFF,
-                            #"frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                        }
-                        )
+                          task_kwargs={
+                              "z": client.active_client.TAKEOFF_HEIGHT,
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "safe_takeoff": client.active_client.SAFE_TAKEOFF,
+                              # "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          }
+                          )
 
     rfp_time = start_time + client.active_client.TAKEOFF_TIME
     task_manager.add_task(rfp_time, 0, animation.execute_frame,
-                        task_kwargs={
-                            "point": animation.convert_frame(frames[0])[0],
-                            "color": animation.convert_frame(frames[0])[1],
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                            "flight_func": FlightLib.reach_point,
-                        }
-                        )
+                          task_kwargs={
+                              "point": animation.convert_frame(frames[0])[0],
+                              "color": animation.convert_frame(frames[0])[1],
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                              "flight_func": FlightLib.reach_point,
+                          }
+                          )
 
     frame_time = rfp_time + client.active_client.RFP_TIME
     frame_delay = 0.125  # TODO from animation file
     for frame in frames:
+        point, color, yaw = animation.convert_frame(frame)  # TODO add param to calculate delta
         task_manager.add_task(frame_time, 0, animation.execute_frame,
-                        task_kwargs={
-                            "point": animation.convert_frame(frame)[0],
-                            "color": animation.convert_frame(frame)[1],
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                            "flight_func": FlightLib.navto,
-                            }
-                        )
+                              task_kwargs={
+                                  "point": point,
+                                  "color": color,
+                                  "frame_id": client.active_client.FRAME_ID,
+                                  "use_leds": client.active_client.USE_LEDS,
+                                  "flight_func": FlightLib.navto,
+                              }
+                              )
         frame_time += frame_delay
 
     land_time = frame_time + client.active_client.LAND_TIME
     task_manager.add_task(land_time, 0, animation.land,
-                        task_kwargs={
-                            "timeout": client.active_client.TAKEOFF_TIME,
-                            "frame_id": client.active_client.FRAME_ID,
-                            "use_leds": client.active_client.USE_LEDS,
-                        },
-                        )
+                          task_kwargs={
+                              "timeout": client.active_client.TAKEOFF_TIME,
+                              "frame_id": client.active_client.FRAME_ID,
+                              "use_leds": client.active_client.USE_LEDS,
+                          },
+                          )
 
 
 if __name__ == "__main__":
-    
     copter_client = CopterClient()
     task_manager = tasking.TaskManager()
-    
+
     copter_client.start(task_manager)
 
-    #ros_logging.route_logger_to_ros()
-    #ros_logging.route_logger_to_ros("__main__")
-    #ros_logging.route_logger_to_ros("client")
-    #ros_logging.route_logger_to_ros("messaging")
-
+    # ros_logging.route_logger_to_ros()
+    # ros_logging.route_logger_to_ros("__main__")
+    # ros_logging.route_logger_to_ros("client")
+    # ros_logging.route_logger_to_ros("messaging")

Drone/tasking_lib.py

@@ -10,7 +10,9 @@ Task = collections.namedtuple("Task", ["func", "args", "kwargs", "delayable", ])
 
 INTERRUPTER = threading.Event()
 
-def wait(end, interrupter=INTERRUPTER, maxsleep=0.1):  # Added features to interrupter sleep and set max sleeping interval
+
+def wait(end, interrupter=INTERRUPTER, maxsleep=0.1):
+    # Added features to interrupter sleep and set max sleeping interval
 
     while not interrupter.is_set():  # Basic implementation of pause module until()
         now = time.time()
@@ -42,7 +44,10 @@ class TaskManager(object):
         self._timeshift = 0.0
 
     def add_task(self, timestamp, priority, task_function,
-                 task_args=(), task_kwargs={}, task_delayable=False):
+                 task_args=(), task_kwargs=None, task_delayable=False):
+
+        if task_kwargs is None:
+            task_kwargs = {}
 
         self._wait_interrupt_event.set()
         self._running_event.clear()
@@ -80,7 +85,7 @@ class TaskManager(object):
                 return heapq.heappop(self.task_queue)
             raise KeyError('Pop from an empty priority queue')
 
-    def start(self, timeouts=False):
+    def start(self):
         #print("Task manager is started")
         #logger.info("Task manager is started")
         self._processor_thread.start()
@@ -92,13 +97,13 @@ class TaskManager(object):
         with self._task_queue_lock:
             del self.task_queue[:]
 
-    def shutdown(self):
+    def shutdown(self, timeout=5.0):
         self.stop()
         self._shutdown_event.set()
-        self._wait_interrupt_event.set()
-        self._task_interrupt_event.set()
-        self._running_event.clear()
-        self._processor_thread.join(timeout=5)
+        # self._wait_interrupt_event.set()
+        # self._task_interrupt_event.set()
+        # self._running_event.clear() #already exists in pause
+        self._processor_thread.join(timeout=timeout)
 
     def pause(self, interrupt=True):
         if interrupt:
@@ -108,7 +113,7 @@ class TaskManager(object):
         #logger.info("Task queue paused")
         #print("Task queue paused")
 
-    def resume(self, time_to_start_next_task = 0):
+    def resume(self, time_to_start_next_task=0.0):
         if self.task_queue:
             next_task_time = self.task_queue[0][0]
             if time_to_start_next_task > next_task_time:

Server/copter_table_models.py

@@ -0,0 +1,263 @@
+import sys
+import re
+import collections
+
+from PyQt5 import QtCore, QtGui, QtWidgets
+from PyQt5.QtCore import Qt as Qt
+
+
+class CopterData:
+    class_attrs = collections.OrderedDict([('copter_id', None), ('anim_id', None), ('batt_v', None), ('batt_p', None),
+                                           ('selfcheck', None), ('time_utc', None), ("time_delta", None),
+                                           ("client", None), ("checked", 0)], )
+
+    def __init__(self, **kwargs):
+        self.attrs = self.class_attrs.copy()
+        self.attrs.update(kwargs)
+
+        for attr, value in self.attrs.items():
+            setattr(self, attr, value)
+
+    def __getitem__(self, key):
+        return getattr(self, list(self.attrs.keys())[key])
+
+    def __setitem__(self, key, value):
+        setattr(self, list(self.attrs.keys())[key], value)
+
+
+class CopterDataModel(QtCore.QAbstractTableModel):
+    checks = {}
+    selected_ready_signal = QtCore.pyqtSignal(bool)
+    selected_takeoff_ready_signal = QtCore.pyqtSignal(bool)
+
+    def __init__(self, parent=None):
+        super(CopterDataModel, self).__init__(parent)
+        self.headers = ('copter ID', 'animation ID', 'battery V', 'battery %', 'selfcheck', 'time delta')
+        self.data_contents = []
+
+    def insertRows(self, contents, position='last', parent=QtCore.QModelIndex()):
+        rows = len(contents)
+        position = len(self.data_contents) if position == 'last' else position
+
+        self.beginInsertRows(parent, position, position + rows - 1)
+        self.data_contents[position:position] = contents
+
+        self.endInsertRows()
+
+    def user_selected(self):
+        return filter(lambda x: x.checked == Qt.Checked, self.data_contents)
+
+    def selfchecked_ready(self, contents=()):
+        contents = contents or self.data_contents
+        return filter(lambda x: all_checks(x), contents)
+
+    def takeoff_ready(self, contents=()):
+        contents = contents or self.data_contents
+        return filter(lambda x: takeoff_checks(x), contents)
+
+    def rowCount(self, n=None):
+        return len(self.data_contents)
+
+    def columnCount(self, n=None):
+        return len(self.headers)
+
+    def headerData(self, section, orientation, role=Qt.DisplayRole):
+        if role == Qt.DisplayRole:
+            if orientation == Qt.Horizontal:
+                return self.headers[section]
+
+    def data(self, index, role=Qt.DisplayRole):
+        row = index.row()
+        col = index.column()
+        #print('row {}, col {}, role {}'.format(row, col, role))
+        if role == Qt.DisplayRole:
+            #print(self.data_contents[row][col])
+            return self.data_contents[row][col] or ""
+
+        elif role == Qt.BackgroundRole:
+            if col in self.checks.keys():
+                item = self.data_contents[row][col]
+                result = self.checks[col](item)
+                if result is None:
+                    return QtGui.QBrush(Qt.yellow)
+                if result:
+                    return QtGui.QBrush(Qt.green)
+                else:
+                    return QtGui.QBrush(Qt.red)
+
+        elif role == Qt.CheckStateRole and col == 0:
+            return self.data_contents[row].checked
+
+    def update_model(self, index=QtCore.QModelIndex()):
+        #self.modelReset.emit()
+        self.selected_ready_signal.emit(set(self.user_selected()).issubset(self.selfchecked_ready()))
+        self.selected_takeoff_ready_signal.emit(set(self.user_selected()).issubset(self.takeoff_ready()))
+        self.dataChanged.emit(index, index, (QtCore.Qt.EditRole,))
+
+    @QtCore.pyqtSlot()
+    def setData(self, index, value, role=Qt.EditRole):
+        if not index.isValid():
+            return False
+
+        if role == Qt.CheckStateRole:
+            self.data_contents[index.row()].checked = value
+
+        elif role == Qt.EditRole:
+            self.data_contents[index.row()][index.column()] = value
+            self.update_model(index)
+        else:
+            return False
+
+        return True
+
+    def flags(self, index):
+        roles = Qt.ItemIsSelectable | Qt.ItemIsEnabled
+        if index.column() == 0:
+            roles |= Qt.ItemIsUserCheckable
+        return roles
+
+    @QtCore.pyqtSlot(int, int, QtCore.QVariant)
+    def update_item(self, row, col, value):
+        self.setData(self.index(row, col), value)
+
+    @QtCore.pyqtSlot(object)
+    def add_client(self, client):
+        self.insertRows([client])
+
+
+def col_check(col):
+    def inner(f):
+        CopterDataModel.checks[col] = f
+
+        def wrapper(*args, **kwargs):
+            return f(*args, **kwargs)
+
+        return wrapper
+
+    return inner
+
+
+@col_check(1)
+def check_anim(item):
+    if not item:
+        return None
+    if str(item) == 'No animation':
+        return False
+    else:
+        return True
+
+
+@col_check(2)
+def check_bat_v(item):
+    if not item:
+        return None
+    if float(item) > 3.2:  # todo config
+        return True
+    else:
+        return False
+
+
+@col_check(3)
+def check_bat_p(item):
+    if not item:
+        return None
+    if float(item) > 30:  # todo config
+        return True
+    else:
+        return False
+        #return True #For testing
+
+
+@col_check(4)
+def check_selfcheck(item):
+    if not item:
+        return None
+    if item == "OK":
+        return True
+    else:
+        return False
+
+@col_check(5)
+def check_time_delta(item):
+    if not item:
+        return None
+    if abs(float(item)) < 1:
+        return True
+    else:
+        return False
+
+
+def all_checks(copter_item):
+    for col, check in CopterDataModel.checks.items():
+        if not check(copter_item[col]):
+            return False
+    return True
+
+def takeoff_checks(copter_item):
+    for i in range(3):
+        if not CopterDataModel.checks[2+i](copter_item[2+i]):
+            return False
+    return True
+
+class CopterProxyModel(QtCore.QSortFilterProxyModel):
+    def __init__(self, parent=None):
+        super(CopterProxyModel, self).__init__(parent)
+
+    @staticmethod
+    def human_sort_prepare(item):
+        if item:
+            item = [int(x) if x.isdigit() else x.lower() for x in re.split('([0-9]+)', str(item))]
+        else:
+            item = []
+        return item
+
+    def lessThan(self, left, right):
+        leftData = self.sourceModel().data(left)
+        rightData = self.sourceModel().data(right)
+
+        return self.human_sort_prepare(leftData) < self.human_sort_prepare(rightData)
+
+
+class SignalManager(QtCore.QObject):
+    update_data_signal = QtCore.pyqtSignal(int, int, QtCore.QVariant)
+    add_client_signal = QtCore.pyqtSignal(object)
+
+
+if __name__ == '__main__':
+    import threading
+    import time
+
+    def timer():
+        idc = 1001
+        while True:
+            myModel.setData(myModel.index(0, 0), idc)
+            idc += 1
+            time.sleep(1)
+
+    app = QtWidgets.QApplication.instance()
+    if app is None:
+        app = QtWidgets.QApplication(sys.argv)
+    tableView = QtWidgets.QTableView()
+    myModel = CopterDataModel(None)
+    proxyModel = CopterProxyModel()
+
+    proxyModel.setDynamicSortFilter(True)
+    proxyModel.setSourceModel(myModel)
+
+    tableView.setModel(proxyModel)
+
+    tableView.verticalHeader().hide()
+    tableView.setSortingEnabled(True)
+
+    tableView.show()
+    myModel.add_client(CopterData(copter_id=1000, checked=0, time_utc=1))
+    myModel.add_client(CopterData(checked=2, selfcheck="OK", time_utc=2))
+    myModel.add_client(CopterData(checked=2, selfcheck="not ok", time_utc="no"))
+
+    myModel.setData(myModel.index(0, 1), "test")
+
+    t = threading.Thread(target=timer, daemon=True)
+    t.start()
+    print(QtCore.QT_VERSION_STR)
+
+    app.exec_()

Server/server.py

@@ -140,7 +140,7 @@ class Server:
         self.sel.register(self.server_socket, selectors.EVENT_READ | selectors.EVENT_WRITE, data=None)
 
         while self.client_processor_thread_running.is_set():
-            events = self.sel.select(timeout=0)
+            events = self.sel.select()
             for key, mask in events:
                 if key.data is None:
                     self._connect_client(key.fileobj)
@@ -159,7 +159,7 @@ class Server:
         logging.info("Got connection from: {}".format(str(addr)))
         conn.setblocking(False)
 
-        if not any(client_addr == addr[0] for client_addr in Client.clients.keys()):
+        if not any([client_addr == addr[0] for client_addr in Client.clients.keys()]):
             client = Client(addr[0])
             logging.info("New client")
         else:

Server/server_qt.py

@@ -12,22 +12,73 @@ from PyQt5.QtWidgets import QFileDialog, QMessageBox
 from server_gui import Ui_MainWindow
 
 from server import *
+from copter_table_models import *
 from emergency import *
 
+
+def confirmation_required(text="Are you sure?", label="Confirm operation?"):
+    def inner(f):
+
+        def wrapper(*args, **kwargs):
+            reply = QMessageBox.question(
+                args[0], label,
+                text,
+                QMessageBox.Yes | QMessageBox.No, QMessageBox.No
+            )
+            if reply == QMessageBox.Yes:
+                print("Dialog accepted")
+                #print(args)
+                return f(args[0])
+            else:
+                print("Dialog declined")
+
+        return wrapper
+
+    return inner
+
+
 # noinspection PyArgumentList,PyCallByClass
 class MainWindow(QtWidgets.QMainWindow):
     def __init__(self):
         super(MainWindow, self).__init__()
+
         self.ui = Ui_MainWindow()
         self.ui.setupUi(self)
         self.init_ui()
+
+        self.model = CopterDataModel()
+        self.proxy_model = CopterProxyModel()
+        self.signals = SignalManager()
+
+        self.init_model()
+        
         self.show()
+        
+    def init_model(self):
+        self.proxy_model.setDynamicSortFilter(True)
+        self.proxy_model.setSourceModel(self.model)
+
+        # Initiate table and table self.model
+        self.ui.tableView.setModel(self.proxy_model)
+        self.ui.tableView.horizontalHeader().setStretchLastSection(True)
+        self.ui.tableView.setSortingEnabled(True)
+
+        # Connect signals to manipulate model from threads
+        self.signals.update_data_signal.connect(self.model.update_item)
+        self.signals.add_client_signal.connect(self.model.add_client)
+
+        # Connect model signals to UI
+        self.model.selected_ready_signal.connect(self.ui.start_button.setEnabled)
+        self.model.selected_takeoff_ready_signal.connect(self.ui.takeoff_button.setEnabled)
+
+    def client_connected(self, client: Client):
+        self.signals.add_client_signal.emit(CopterData(copter_id=client.copter_id, client=client))
 
     def init_ui(self):
         # Connecting
-        self.ui.check_button.clicked.connect(self.check_selected)
+        self.ui.check_button.clicked.connect(self.selfcheck_selected)
         self.ui.start_button.clicked.connect(self.send_starttime)
-        self.ui.pause_button.clicked.connect(self.pause_all)
+        self.ui.pause_button.clicked.connect(self.pause_resume_selected)
         self.ui.stop_button.clicked.connect(self.stop_all)
         self.ui.emergency_button.clicked.connect(self.emergency)
 
@@ -40,103 +91,92 @@ class MainWindow(QtWidgets.QMainWindow):
         self.ui.action_send_configurations.triggered.connect(self.send_configurations)
         self.ui.action_send_Aruco_map.triggered.connect(self.send_aruco)
 
-        # Initing table and table model
-        self.ui.tableView.setModel(model)
-        self.ui.tableView.horizontalHeader().setStretchLastSection(True)
-        # self.ui = Communicate()
-        # self.ui.button.connect(self.button1)
+        # Set most safety-important buttons disabled
+        self.ui.start_button.setEnabled(False)
+        self.ui.takeoff_button.setEnabled(False)
 
     @pyqtSlot()
-    def check_selected(self):
-        for row_num in range(model.rowCount()):
-            item = model.item(row_num, 0)
-            if item.isCheckable() and item.checkState() == Qt.Checked:
-                print("Copter {} checked".format(model.item(row_num, 0).text()))
-                copter = Client.get_by_id(item.text())
-                copter.get_response("anim_id", self._set_copter_data, callback_args=(row_num, 1))
-                copter.get_response("batt_voltage", self._set_copter_data, callback_args=(row_num, 2))
-                copter.get_response("cell_voltage", self._set_copter_data, callback_args=(row_num, 3))
-                copter.get_response("selfcheck", self._set_copter_data, callback_args=(row_num, 4))
-                copter.get_response("time", self._set_copter_data, callback_args=(row_num, 5))
+    def selfcheck_selected(self):
+        for copter in self.model.user_selected():
+            client = copter.client
 
-        self.ui.start_button.setEnabled(True)
-        self.ui.takeoff_button.setEnabled(True)
+            client.get_response("anim_id", self._set_copter_data, callback_args=(1, copter.copter_id))
+            client.get_response("batt_voltage", self._set_copter_data, callback_args=(2, copter.copter_id))
+            client.get_response("cell_voltage", self._set_copter_data, callback_args=(3, copter.copter_id))
+            client.get_response("selfcheck", self._set_copter_data, callback_args=(4, copter.copter_id))
+            client.get_response("time", self._set_copter_data, callback_args=(5, copter.copter_id))
+
+    def _set_copter_data(self, value, col, copter_id):
+        row = self.model.data_contents.index(next(
+            filter(lambda x: x.copter_id == copter_id, self.model.data_contents)))
 
-    def _set_copter_data(self, value, row, col):
         if col == 1:
-            model.setData(model.index(row, col), value)
+            data = value
         elif col == 2:
-            model.setData(model.index(row, col), "{} V".format(round(float(value), 3)))
+            data = "{}".format(round(float(value), 3))
         elif col == 3:
-            batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100
-            model.setData(model.index(row, col), "{} %".format(round(batt_percent, 3)))
+            batt_percent = ((float(value) - 3.2) / (4.2 - 3.2)) * 100  # TODO config
+            data = "{}".format(round(batt_percent, 3))
         elif col == 4:
-            if value != "OK":
-                model.setData(model.index(row, col), str(value))  # TODO different handling
-            else:
-                model.setData(model.index(row, col), str(value))
+            data = str(value)
         elif col == 5:
-            model.setData(model.index(row, col), time.ctime(int(value)))
+            #data = time.ctime(int(value))
+            data = "{}".format(round(float(value) - time.time(), 3))
+            if abs(float(data)) > 1:
+                Client.get_by_id(copter_id).send_message("repair_chrony")
+            #self.signals.update_data_signal.emit(row, col + 1, data2)
+        else:
+            print("No column matched for response")
+            return
+
+        self.signals.update_data_signal.emit(row, col, data)
+
+    @confirmation_required("This operation will takeoff selected copters with delay and start animation. Proceed?")
+    @pyqtSlot()
+    def send_starttime(self, **kwargs):
+        dt = self.ui.start_delay_spin.value()
+        for copter in self.model.user_selected():
+            if all_checks(copter):
+                server.send_starttime(copter.client, dt)
+
+    @confirmation_required("This operation will takeoff copters immediately. Proceed?")
+    @pyqtSlot()
+    def takeoff_selected(self, **kwargs):
+        for copter in self.model.user_selected():
+            if all_checks(copter):
+                copter.client.send_message("takeoff")
+
+    @confirmation_required("This operation will flip(!!!) copters immediately. Proceed?")
+    @pyqtSlot()
+    def flip(self, **kwargs):
+        for copter in self.model.user_selected():
+            if all_checks(copter):
+                copter.client.send_message("flip")
 
     @pyqtSlot()
-    def send_starttime(self):
-        dt = self.ui.start_delay_spin.value()
-        reply = QMessageBox.question(
-            self, "Confirm operation",
-            "This operation will takeoff selected copters and start animation after {} seconds. Proceed?".format(dt),
-            QMessageBox.Yes | QMessageBox.No, QMessageBox.No
-        )
-        if reply == QMessageBox.Yes:
-            print("Accepted")
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    if True:  # TODO checks for batt/selfckeck here
-                        copter = Client.get_by_id(item.text())
-                        server.send_starttime(copter, dt)
-        else:
-            print("Cancelled")
+    def test_leds(self):
+        for copter in self.model.user_selected():
+            copter.client.send_message("led_test")
 
     @pyqtSlot()
     def stop_all(self):
         Client.broadcast_message("stop")
 
     @pyqtSlot()
-    def pause_all(self):
+    def pause_resume_selected(self):
         if self.ui.pause_button.text() == 'Pause':
-            Client.broadcast_message('pause')
+            for copter in self.model.user_selected():
+                copter.client.send_message("pause")
             self.ui.pause_button.setText('Resume')
         else:
-            Client.broadcast_message('resume')
-            self.ui.pause_button.setText('Pause')
+            self._resume_selected()
 
-    @pyqtSlot()
-    def test_leds(self):
-        for row_num in range(model.rowCount()):
-            item = model.item(row_num, 0)
-            if item.isCheckable() and item.checkState() == Qt.Checked:
-                if True:  # TODO checks for batt/selfckeck here
-                    copter = Client.get_by_id(item.text())
-                    copter.send_message("led_test")
-
-    @pyqtSlot()
-    def takeoff_selected(self):
-        reply = QMessageBox.question(
-            self, "Confirm operation",
-            "This operation will takeoff copters immediately. Proceed?",
-            QMessageBox.Yes | QMessageBox.No, QMessageBox.No
-        )
-        if reply == QMessageBox.Yes:
-            print("Accepted")
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    if True:  # TODO checks for batt/selfckeck here
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("takeoff")
-        else:
-            print("Cancelled")
-            pass
+    #@confirmation_required("This operation will resume ALL copter tasks with given delay. Proceed?")
+    def _resume_selected(self, **kwargs):
+        time_gap = 0.1
+        for copter in self.model.user_selected():
+            copter.client.send_message('resume', {"time": server.time_now() + time_gap})
+        self.ui.pause_button.setText('Pause')
 
     @pyqtSlot()
     def land_all(self):
@@ -156,14 +196,11 @@ class MainWindow(QtWidgets.QMainWindow):
             names = [os.path.basename(file).split(".")[0] for file in files]
             print(files)
             for file, name in zip(files, names):
-                for row_num in range(model.rowCount()):
-                    item = model.item(row_num, 0)
-                    if item.isCheckable() and item.checkState() == Qt.Checked:
-                        copter = Client.get_by_id(item.text())
-                        if name == copter.copter_id:
-                            copter.send_file(file, "animation.csv")  # TODO config
-                        else:
-                            print("Filename not matches with any drone connected")
+                for copter in self.model.user_selected():
+                    if name == copter.copter_id:
+                        copter.client.send_file(file, "animation.csv")  # TODO config
+                else:
+                    print("Filename has no matches with any drone selected")
 
     @pyqtSlot()
     def send_configurations(self):
@@ -178,11 +215,9 @@ class MainWindow(QtWidgets.QMainWindow):
                     value = sendable_config[section][option]
                     logging.debug("Got item from config:".format(section, option, value))
                     options.append(ConfigOption(section, option, value))
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    copter = Client.get_by_id(item.text())
-                    copter.send_config_options(*options)
+
+            for copter in self.model.user_selected():
+                copter.client.send_config_options(*options)
 
     @pyqtSlot()
     def send_aruco(self):
@@ -190,102 +225,64 @@ class MainWindow(QtWidgets.QMainWindow):
         if path:
             filename = os.path.basename(path)
             print("Selected file:", path, filename)
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    copter = Client.get_by_id(item.text())
-                    copter.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt")
-                    copter.send_message("service_restart", {"name": "clever"})
+            for copter in self.model.user_selected():
+                copter.client.send_file(path, "/home/pi/catkin_ws/src/clever/aruco_pose/map/animation_map.txt")
+                copter.client.send_message("service_restart", {"name": "clever"})
+
     @pyqtSlot()
     def emergency(self):
         client_row_min = 0
-        client_row_max = model.rowCount() - 1
+        client_row_max = self.model.rowCount() - 1
         result = -1
-        while (result!=0) and (result != 3) and (result != 4):
+        while (result != 0) and (result != 3) and (result != 4):
             # light_green_red(min, max)
             client_row_mid = int(math.ceil((client_row_max+client_row_min) / 2.0))
             print(client_row_min, client_row_mid, client_row_max)
             for row_num in range(client_row_min, client_row_mid):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("led_fill", {"green": 255})
+                self.model.data_contents[row_num].client\
+                    .send_message("led_fill", {"green": 255})
             for row_num in range(client_row_mid, client_row_max + 1):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("led_fill", {"red": 255})
+                self.model.data_contents[row_num].client \
+                    .send_message("led_fill", {"red": 255})
+
             Dialog = QtWidgets.QDialog()    
             ui = Ui_Dialog()
             ui.setupUi(Dialog)
             Dialog.show()
             result = Dialog.exec()
             print("Dialog result: {}".format(result))
-            if (client_row_max != client_row_min):
-                if (result == 1): 
+
+            if client_row_max != client_row_min:
+                if result == 1:
                     for row_num in range(client_row_mid, client_row_max + 1):
-                        item = model.item(row_num, 0)
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("led_fill")
+                        self.model.data_contents[row_num].client \
+                            .send_message("led_fill")
                     client_row_max = client_row_mid - 1
                    
-                elif (result == 2):
+                elif result == 2:
                     for row_num in range(client_row_min, client_row_mid):
-                        item = model.item(row_num, 0)
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("led_fill")
+                        self.model.data_contents[row_num].client \
+                            .send_message("led_fill")
                     client_row_min = client_row_mid
 
         if result == 0:
             Client.broadcast_message("led_fill")
         elif result == 3:
             for row_num in range(client_row_min, client_row_max + 1):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("land")
+                self.model.data_contents[row_num].client \
+                    .send_message("land")
         elif result == 4:
             for row_num in range(client_row_min, client_row_max + 1):
-                item = model.item(row_num, 0)
-                copter = Client.get_by_id(item.text())
-                copter.send_message("disarm")
- 
-    @pyqtSlot()    
-    def flip(self):
-        reply = QMessageBox.question(
-            self, "Confirm operation",
-            "You are ready to turn the copter?",
-            QMessageBox.Yes | QMessageBox.No, QMessageBox.No
-        )
-        if reply == QMessageBox.Yes:
-            print("Accepted")
-            for row_num in range(model.rowCount()):
-                item = model.item(row_num, 0)
-                if item.isCheckable() and item.checkState() == Qt.Checked:
-                    if True:  # TODO checks for batt/selfckeck here
-                        copter = Client.get_by_id(item.text())
-                        copter.send_message("flip")
-        else:
-            print("Cancelled")
-            pass
-model = QStandardItemModel()
-model.setHorizontalHeaderLabels(
-    ('copter ID', 'animation ID', 'battery V', 'battery %', 'selfcheck', 'time UTC')
-)
-model.setColumnCount(6)
-model.setRowCount(0)
-
-
-def client_connected(self: Client):
-    copter_id_item = QStandardItem(self.copter_id)
-    copter_id_item.setCheckable(True)
-    model.appendRow((copter_id_item, ))
-
-
-Client.on_first_connect = client_connected
+                self.model.data_contents[row_num].client \
+                    .send_message("disarm")
 
 
 if __name__ == "__main__":
     app = QtWidgets.QApplication(sys.argv)
     window = MainWindow()
 
+    Client.on_first_connect = window.client_connected
+
     server = Server(on_stop=app.quit)
     server.start()
 

builder/assets/clever-show.service

@@ -1,12 +1,12 @@
 [Unit]
 Description=Clever Show Client Service
-Requires=clever.service
 After=clever.service
 
 [Service]
 WorkingDirectory=/home/pi/CleverSwarm/Drone
 EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/usr/bin/python /home/pi/CleverSwarm/Drone/copter_client.py
+KillSignal=SIGKILL
 Restart=on-failure
 RestartSec=3
 

builder/image-configure.sh

@@ -44,7 +44,7 @@ sed -i '/<arg name="aruco_vpe"/c \    <arg name="aruco_vpe" default="true"/>' /h
 sed -i '/<param name="map"/c \        <param name="map" value="\$\(find aruco_pose\)/map/animation_map.txt"/>' /home/pi/catkin_ws/src/clever/clever/launch/aruco.launch
 sed -i '/<arg name="aruco"/c \    <arg name="aruco" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
 sed -i '/<arg name="rangefinder_vl53l1x"/c \    <arg name="rangefinder_vl53l1x" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
-#sed -i '/<arg name="optical_flow"/c \    <arg name="optical_flow" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
+sed -i '/<arg name="optical_flow"/c \    <arg name="optical_flow" default="true"/>' /home/pi/catkin_ws/src/clever/clever/launch/clever.launch
 
 echo_stamp "Image was configured!" "SUCCESS"