From 48369744a78ef79d7f2d02b929a9ae607f733db5 Mon Sep 17 00:00:00 2001
From: tajgr <levjakub@gmail.com>
Date: Wed, 25 Oct 2023 23:26:51 +0200
Subject: [PATCH 1/7] Move files from subt to osgar

---
 osgar/drivers/pull.py                     |  51 +++++
 osgar/drivers/push.py                     |  56 +++++
 osgar/lib/local_planner.py                | 248 ++++++++++++++++++++++
 {subt => osgar/lib}/test_local_planner.py |   0
 4 files changed, 355 insertions(+)
 create mode 100644 osgar/drivers/pull.py
 create mode 100644 osgar/drivers/push.py
 create mode 100644 osgar/lib/local_planner.py
 rename {subt => osgar/lib}/test_local_planner.py (100%)

diff --git a/osgar/drivers/pull.py b/osgar/drivers/pull.py
new file mode 100644
index 000000000..7ab13979e
--- /dev/null
+++ b/osgar/drivers/pull.py
@@ -0,0 +1,51 @@
+import contextlib
+
+from threading import Thread
+
+import zmq
+
+from osgar.bus import BusShutdownException
+import osgar.lib.serialize
+
+class Pull:
+
+    def __init__(self, config, bus):
+        bus.register(*config['outputs'])
+        self.is_bind_set = config.get("bind", False)
+        self.endpoint = config.get('endpoint', 'tcp://127.0.0.1:5565')
+        self.timeout = config.get('timeout', 1) # default recv timeout 1s
+        self.thread = Thread(target=self.run)
+        self.thread.name = bus.name
+        self.bus = bus
+
+    def start(self):
+        self.thread.start()
+
+    def join(self, timeout=None):
+        self.thread.join(timeout=timeout)
+
+    def run(self):
+        context = zmq.Context.instance()
+        socket = context.socket(zmq.PULL)
+        # https://stackoverflow.com/questions/7538988/zeromq-how-to-prevent-infinite-wait
+        socket.RCVTIMEO = int(self.timeout * 1000)  # convert to milliseconds
+
+        if self.is_bind_set:
+            socket.LINGER = 100
+            socket.bind(self.endpoint)
+        else:
+            socket.connect(self.endpoint)
+
+        with contextlib.closing(socket):
+            while self.bus.is_alive():
+                try:
+                    channel, raw = socket.recv_multipart()
+                    message = osgar.lib.serialize.deserialize(raw)
+                    self.bus.publish(channel.decode('ascii'), message)
+                except zmq.error.Again:
+                    pass
+
+    def request_stop(self):
+        self.bus.shutdown()
+
+
diff --git a/osgar/drivers/push.py b/osgar/drivers/push.py
new file mode 100644
index 000000000..046574c88
--- /dev/null
+++ b/osgar/drivers/push.py
@@ -0,0 +1,56 @@
+import contextlib
+
+from threading import Thread
+
+import zmq
+
+from osgar.bus import BusShutdownException
+from osgar.lib.serialize import serialize
+
+
+class Push:
+
+    def __init__(self, config, bus):
+        bus.register()
+        self.is_bind_set = config.get("bind", False)
+        self.endpoint = config.get('endpoint', 'tcp://127.0.0.1:5566')
+        self.timeout = config.get('timeout', 1) # default send timeout 1s, effective on full queue only
+        self.thread = Thread(target=self.run)
+        self.thread.name = bus.name
+        self.bus = bus
+
+    def start(self):
+        self.thread.start()
+
+    def join(self, timeout=None):
+        self.thread.join(timeout=timeout)
+
+    def run(self):
+        context = zmq.Context.instance()
+        socket = context.socket(zmq.PUSH)
+
+        if self.is_bind_set:
+            socket.setsockopt(zmq.LINGER, 100)  # milliseconds
+            socket.bind(self.endpoint)
+        else:
+            # https://stackoverflow.com/questions/7538988/zeromq-how-to-prevent-infinite-wait
+            socket.SNDTIMEO = int(self.timeout * 1000)  # convert to milliseconds
+            socket.LINGER = 100 #milliseconds
+            socket.connect(self.endpoint)
+
+        try:
+            with contextlib.closing(socket):
+                while True:
+                    dt, channel, data = self.bus.listen()
+                    raw = serialize(data)
+                    socket.send_multipart([bytes(channel, 'ascii'), raw])
+        except zmq.ZMQError as e:
+            if e.errno == zmq.EAGAIN:
+                pass #TODO log timeout
+            else:
+                pass #TODO log other error
+        except BusShutdownException:
+            pass
+
+    def request_stop(self):
+        self.bus.shutdown()
diff --git a/osgar/lib/local_planner.py b/osgar/lib/local_planner.py
new file mode 100644
index 000000000..e76c4403b
--- /dev/null
+++ b/osgar/lib/local_planner.py
@@ -0,0 +1,248 @@
+import math
+import numpy as np
+import cProfile
+
+
+g_pr = cProfile.Profile()
+g_count = 0
+
+
+def normalize_angle(angle):
+    return (angle + math.pi) % (2 * math.pi) - math.pi
+
+
+class LocalPlannerRef:
+    def __init__(self, scan_start=math.radians(-135), scan_end=math.radians(135), direction_adherence=math.radians(90),
+                 max_obstacle_distance=1.5, obstacle_influence=1.2, scan_subsample=1, max_considered_obstacles=None):
+        self.last_scan = None
+        self.scan_start = scan_start
+        self.scan_end = scan_end
+        self.direction_adherence = direction_adherence
+        self.max_obstacle_distance = max_obstacle_distance
+        self.obstacle_influence = obstacle_influence
+        self.scan_subsample = scan_subsample
+
+    def update(self, scan):
+        self.last_scan = scan[::self.scan_subsample] if self.scan_subsample > 1 else scan
+
+    def recommend(self, desired_dir):
+        if self.last_scan is None:
+            return 1.0, desired_dir
+
+        obstacles = []
+        for (i, measurement) in enumerate(self.last_scan):
+            if measurement == 0:
+                continue
+            if measurement * 1e-3 > self.max_obstacle_distance:
+                continue
+            measurement_angle = self.scan_start + (self.scan_end - self.scan_start) * i / float(len(self.last_scan) - 1)
+            measurement_vector = math.cos(measurement_angle), math.sin(measurement_angle)
+
+            # Converting from millimeters to meters.
+            obstacle_xy = [mv * (measurement * 1e-3) for mv in measurement_vector]
+
+            obstacles.append(obstacle_xy)
+
+        if not obstacles:
+            return 1.0, normalize_angle(desired_dir)
+
+        # Best direction points roughly in desired_dir and does not come too close to any obstacle.
+        def is_desired(direction):
+            direction_delta = normalize_angle(direction - desired_dir)
+            return math.exp(-(direction_delta / self.direction_adherence)**2)  # Fuzzy equality
+
+        def is_risky(direction):
+            direction_vector = math.cos(direction), math.sin(direction)
+            riskiness = 0.
+            for obstacle_xy in obstacles:
+                # Obstacles behind the robot from the perspective of the desired direction do not matter.
+                # The formula below computes cos(angle between the two vectors) * their_norms. Norms are positive, so a negative result implies abs(angle) > 90deg.
+                if sum(d * o for (d, o) in zip(direction_vector, obstacle_xy)) < 0:
+                    continue
+
+                # Distance between the obstacle and line defined by direction.
+                # https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
+                # Norm of direction_vector is 1.0, so we do not need to divide by it.
+                off_track_distance = abs(direction_vector[1] * obstacle_xy[0] - direction_vector[0] * obstacle_xy[1])
+
+                r = math.exp(-(off_track_distance / self.obstacle_influence)**2)
+                if r > riskiness: # max as fuzzy OR.
+                    riskiness = r
+
+            return riskiness
+
+        def is_safe(direction):
+            return 1.0 - is_risky(direction)  # Fuzzy negation.
+
+        def is_good(direction):
+            return min(is_safe(direction), is_desired(direction))  # Fuzzy AND.
+
+        return max((is_good(math.radians(direction)), math.radians(direction)) for direction in range(-180, 180, 3))
+
+
+class LocalPlannerOpt:
+    """
+    Optimized version of Local Planner.
+    1) ignore all directions into obstacles - there is always a way out (lidar is 270 degrees so it can go freely back)
+       and way into the obstacle will have high penalization cost.
+    2) leave complex evaluation as the last step for already best direction
+    """
+    def __init__(self, scan_start=math.radians(-135), scan_end=math.radians(135), direction_adherence=math.radians(90),
+                 max_obstacle_distance=1.5, obstacle_influence=1.2, scan_subsample=1, max_considered_obstacles=None):
+        self.last_scan = None
+        self.scan_start = scan_start
+        self.scan_end = scan_end
+        self.direction_adherence = direction_adherence
+        self.max_obstacle_distance = max_obstacle_distance
+        self.obstacle_influence = obstacle_influence
+        self.scan_subsample = scan_subsample
+
+    def update(self, scan):
+        self.last_scan = scan[::self.scan_subsample] if self.scan_subsample > 1 else scan
+
+    def recommend(self, desired_dir):
+        if self.last_scan is None:
+            return 1.0, desired_dir
+
+        # valid candidate angles for consideration
+        valid = [True] * 361  # indexed -180 .. 180
+
+        obstacles = []
+        for (i, measurement) in enumerate(self.last_scan):
+            if measurement == 0:
+                continue
+            if measurement * 1e-3 > self.max_obstacle_distance:
+                continue
+            measurement_angle = self.scan_start + (self.scan_end - self.scan_start) * i / float(len(self.last_scan) - 1)
+            measurement_vector = math.cos(measurement_angle), math.sin(measurement_angle)
+
+            # Converting from millimeters to meters.
+            obstacle_xy = [mv * measurement * 1e-3 for mv in measurement_vector]
+
+            obstacles.append(obstacle_xy)
+            valid[int(math.degrees(normalize_angle(measurement_angle))) + 180] = False
+
+#        print('obstacles', len(obstacles))
+        if not obstacles:
+            return 1.0, normalize_angle(desired_dir)
+
+        # Best direction points roughly in desired_dir and does not come too close to any obstacle.
+        def is_desired(direction):
+            direction_delta = normalize_angle(direction - desired_dir)
+            return math.exp(-(direction_delta / self.direction_adherence)**2)  # Fuzzy equality
+
+        def is_risky(direction):
+            direction_vector = math.cos(direction), math.sin(direction)
+            min_off_track_distance = 1000.
+            for obstacle_xy in obstacles:
+                # Obstacles behind the robot from the perspective of the desired direction do not matter.
+                # The formula below computes cos(angle between the two vectors) * their_norms. Norms are positive, so a negative result implies abs(angle) > 90deg.
+                if sum(d * o for (d, o) in zip(direction_vector, obstacle_xy)) < 0:
+                    continue
+
+                # Distance between the obstacle and line defined by direction.
+                # https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
+                # Norm of direction_vector is 1.0, so we do not need to divide by it.
+                off_track_distance = abs(direction_vector[1] * obstacle_xy[0] - direction_vector[0] * obstacle_xy[1])
+
+                if off_track_distance < min_off_track_distance: # max as fuzzy OR.
+                    min_off_track_distance = off_track_distance
+
+            riskiness = math.exp(-(min_off_track_distance / self.obstacle_influence)**2)
+            return riskiness
+
+        def is_safe(direction):
+            return 1.0 - is_risky(direction)  # Fuzzy negation.
+
+        def is_good(direction):
+            return min(is_safe(direction), is_desired(direction))  # Fuzzy AND.
+
+        return max((is_good(math.radians(direction)), math.radians(direction)) for direction in range(-180, 180, 3) if valid[direction + 180])
+
+class LocalPlannerNumpy:
+    def __init__(self, scan_start=math.radians(-135), scan_end=math.radians(135), direction_adherence=math.radians(90),
+                 max_obstacle_distance=1.5, obstacle_influence=1.2, scan_subsample=1, max_considered_obstacles=None):
+        self.last_scan = None
+        self.scan_start = scan_start
+        self.scan_end = scan_end
+        self.direction_adherence = direction_adherence
+        self.max_obstacle_distance = max_obstacle_distance
+        self.obstacle_influence = obstacle_influence
+
+        self.considered_directions = np.radians(
+                np.linspace(
+                    -180, +180, 360//3, endpoint=False)).reshape((-1, 1))
+        self.considered_directions_cos = np.cos(self.considered_directions)
+        self.considered_directions_sin = np.sin(self.considered_directions)
+        self.scan_subsample = scan_subsample
+
+        # To be filled in later, once we know how many directions per scan we
+        # receive.
+        self.angles = None
+        self.angles_cos = None
+        self.angles_sin = None
+        self.scan = None
+        self.max_considered_obstacles = max_considered_obstacles
+
+    def update(self, scan):
+        self.scan = np.asarray(scan[::self.scan_subsample] if self.scan_subsample > 1 else scan) * 1e-3
+
+        if self.angles is None or len(self.scan) != self.angles.shape[1]:
+            n = len(self.scan)
+            delta = (self.scan_end - self.scan_start) / (n - 1)
+            self.angles = np.linspace(
+                    self.scan_start,
+                    self.scan_end,
+                    n).reshape((1, -1))
+            self.angles_cos = np.cos(self.angles)
+            self.angles_sin = np.sin(self.angles)
+
+    def recommend(self, desired_dir):
+        if self.scan is None:
+            return 0.0, desired_dir
+        NO_MEASUREMENT = 0
+        is_valid = np.logical_and(
+                self.scan != NO_MEASUREMENT,
+                self.scan <= self.max_obstacle_distance)
+        valid_scan = self.scan[is_valid]
+        is_valid = is_valid.reshape((1, -1))
+        acoss = self.angles_cos[is_valid]
+        asins = self.angles_sin[is_valid]
+        x = acoss * valid_scan
+        y = asins * valid_scan
+
+        if x.shape[0] == 0:
+            return 1.0, normalize_angle(desired_dir)
+
+        if self.max_considered_obstacles is not None and x.shape[0] > self.max_considered_obstacles:
+            nth = math.ceil(x.shape[0] / self.max_considered_obstacles)
+            x = x[::nth]
+            y = y[::nth]
+
+        num_considered_directions = self.considered_directions.shape[0]
+        xs = np.broadcast_to(x, (num_considered_directions, x.shape[0]))
+        ys = np.broadcast_to(y, (num_considered_directions, y.shape[0]))
+        dist_from_line = np.abs(xs * self.considered_directions_sin -
+                                ys * self.considered_directions_cos)
+        # Obstacles behind the robot from the perspective of the desired direction do not matter.
+        # The formula below computes cos(angle between the two vectors) * their_norms. Norms are positive, so a negative result implies abs(angle) > 90deg.
+        is_in_front = (xs * self.considered_directions_cos +
+                       ys * self.considered_directions_sin) >= 0
+        # Only valid points that can block the movement count.
+        dist_from_line = np.where(is_in_front, dist_from_line, float('inf'))
+        # In each direction, we only care about the nearest obstacle.
+        min_dist = np.min(dist_from_line, axis=1)
+        risky = np.exp(-(min_dist / self.obstacle_influence)**2)
+        safe = 1.0 - risky  # Fuzzy negation.
+
+        direction_delta = normalize_angle(self.considered_directions - desired_dir)
+        desired = np.exp(-(direction_delta/self.direction_adherence)**2).reshape((-1,))  # Fuzzy equality
+
+        good = np.minimum(safe, desired)  # Fuzzy and.
+        best = np.argmax(good)
+        return good[best], self.considered_directions[best][0]
+
+
+LocalPlanner = LocalPlannerNumpy
+
+# vim: expandtab sw=4 ts=4
diff --git a/subt/test_local_planner.py b/osgar/lib/test_local_planner.py
similarity index 100%
rename from subt/test_local_planner.py
rename to osgar/lib/test_local_planner.py

From e5783840172fc8addfd85d6701492f8bfa2575f9 Mon Sep 17 00:00:00 2001
From: tajgr <levjakub@gmail.com>
Date: Thu, 26 Oct 2023 21:25:22 +0200
Subject: [PATCH 2/7] Move files to osgar

---
 {subt => osgar/drivers}/test_odoimuloc.py |   0
 subt/local_planner.py                     | 248 ----------------------
 subt/odoimuloc.py                         |  47 ----
 subt/pull.py                              |  51 -----
 subt/push.py                              |  56 -----
 5 files changed, 402 deletions(-)
 rename {subt => osgar/drivers}/test_odoimuloc.py (100%)
 delete mode 100644 subt/local_planner.py
 delete mode 100644 subt/odoimuloc.py
 delete mode 100644 subt/pull.py
 delete mode 100644 subt/push.py

diff --git a/subt/test_odoimuloc.py b/osgar/drivers/test_odoimuloc.py
similarity index 100%
rename from subt/test_odoimuloc.py
rename to osgar/drivers/test_odoimuloc.py
diff --git a/subt/local_planner.py b/subt/local_planner.py
deleted file mode 100644
index e76c4403b..000000000
--- a/subt/local_planner.py
+++ /dev/null
@@ -1,248 +0,0 @@
-import math
-import numpy as np
-import cProfile
-
-
-g_pr = cProfile.Profile()
-g_count = 0
-
-
-def normalize_angle(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
-
-
-class LocalPlannerRef:
-    def __init__(self, scan_start=math.radians(-135), scan_end=math.radians(135), direction_adherence=math.radians(90),
-                 max_obstacle_distance=1.5, obstacle_influence=1.2, scan_subsample=1, max_considered_obstacles=None):
-        self.last_scan = None
-        self.scan_start = scan_start
-        self.scan_end = scan_end
-        self.direction_adherence = direction_adherence
-        self.max_obstacle_distance = max_obstacle_distance
-        self.obstacle_influence = obstacle_influence
-        self.scan_subsample = scan_subsample
-
-    def update(self, scan):
-        self.last_scan = scan[::self.scan_subsample] if self.scan_subsample > 1 else scan
-
-    def recommend(self, desired_dir):
-        if self.last_scan is None:
-            return 1.0, desired_dir
-
-        obstacles = []
-        for (i, measurement) in enumerate(self.last_scan):
-            if measurement == 0:
-                continue
-            if measurement * 1e-3 > self.max_obstacle_distance:
-                continue
-            measurement_angle = self.scan_start + (self.scan_end - self.scan_start) * i / float(len(self.last_scan) - 1)
-            measurement_vector = math.cos(measurement_angle), math.sin(measurement_angle)
-
-            # Converting from millimeters to meters.
-            obstacle_xy = [mv * (measurement * 1e-3) for mv in measurement_vector]
-
-            obstacles.append(obstacle_xy)
-
-        if not obstacles:
-            return 1.0, normalize_angle(desired_dir)
-
-        # Best direction points roughly in desired_dir and does not come too close to any obstacle.
-        def is_desired(direction):
-            direction_delta = normalize_angle(direction - desired_dir)
-            return math.exp(-(direction_delta / self.direction_adherence)**2)  # Fuzzy equality
-
-        def is_risky(direction):
-            direction_vector = math.cos(direction), math.sin(direction)
-            riskiness = 0.
-            for obstacle_xy in obstacles:
-                # Obstacles behind the robot from the perspective of the desired direction do not matter.
-                # The formula below computes cos(angle between the two vectors) * their_norms. Norms are positive, so a negative result implies abs(angle) > 90deg.
-                if sum(d * o for (d, o) in zip(direction_vector, obstacle_xy)) < 0:
-                    continue
-
-                # Distance between the obstacle and line defined by direction.
-                # https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
-                # Norm of direction_vector is 1.0, so we do not need to divide by it.
-                off_track_distance = abs(direction_vector[1] * obstacle_xy[0] - direction_vector[0] * obstacle_xy[1])
-
-                r = math.exp(-(off_track_distance / self.obstacle_influence)**2)
-                if r > riskiness: # max as fuzzy OR.
-                    riskiness = r
-
-            return riskiness
-
-        def is_safe(direction):
-            return 1.0 - is_risky(direction)  # Fuzzy negation.
-
-        def is_good(direction):
-            return min(is_safe(direction), is_desired(direction))  # Fuzzy AND.
-
-        return max((is_good(math.radians(direction)), math.radians(direction)) for direction in range(-180, 180, 3))
-
-
-class LocalPlannerOpt:
-    """
-    Optimized version of Local Planner.
-    1) ignore all directions into obstacles - there is always a way out (lidar is 270 degrees so it can go freely back)
-       and way into the obstacle will have high penalization cost.
-    2) leave complex evaluation as the last step for already best direction
-    """
-    def __init__(self, scan_start=math.radians(-135), scan_end=math.radians(135), direction_adherence=math.radians(90),
-                 max_obstacle_distance=1.5, obstacle_influence=1.2, scan_subsample=1, max_considered_obstacles=None):
-        self.last_scan = None
-        self.scan_start = scan_start
-        self.scan_end = scan_end
-        self.direction_adherence = direction_adherence
-        self.max_obstacle_distance = max_obstacle_distance
-        self.obstacle_influence = obstacle_influence
-        self.scan_subsample = scan_subsample
-
-    def update(self, scan):
-        self.last_scan = scan[::self.scan_subsample] if self.scan_subsample > 1 else scan
-
-    def recommend(self, desired_dir):
-        if self.last_scan is None:
-            return 1.0, desired_dir
-
-        # valid candidate angles for consideration
-        valid = [True] * 361  # indexed -180 .. 180
-
-        obstacles = []
-        for (i, measurement) in enumerate(self.last_scan):
-            if measurement == 0:
-                continue
-            if measurement * 1e-3 > self.max_obstacle_distance:
-                continue
-            measurement_angle = self.scan_start + (self.scan_end - self.scan_start) * i / float(len(self.last_scan) - 1)
-            measurement_vector = math.cos(measurement_angle), math.sin(measurement_angle)
-
-            # Converting from millimeters to meters.
-            obstacle_xy = [mv * measurement * 1e-3 for mv in measurement_vector]
-
-            obstacles.append(obstacle_xy)
-            valid[int(math.degrees(normalize_angle(measurement_angle))) + 180] = False
-
-#        print('obstacles', len(obstacles))
-        if not obstacles:
-            return 1.0, normalize_angle(desired_dir)
-
-        # Best direction points roughly in desired_dir and does not come too close to any obstacle.
-        def is_desired(direction):
-            direction_delta = normalize_angle(direction - desired_dir)
-            return math.exp(-(direction_delta / self.direction_adherence)**2)  # Fuzzy equality
-
-        def is_risky(direction):
-            direction_vector = math.cos(direction), math.sin(direction)
-            min_off_track_distance = 1000.
-            for obstacle_xy in obstacles:
-                # Obstacles behind the robot from the perspective of the desired direction do not matter.
-                # The formula below computes cos(angle between the two vectors) * their_norms. Norms are positive, so a negative result implies abs(angle) > 90deg.
-                if sum(d * o for (d, o) in zip(direction_vector, obstacle_xy)) < 0:
-                    continue
-
-                # Distance between the obstacle and line defined by direction.
-                # https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
-                # Norm of direction_vector is 1.0, so we do not need to divide by it.
-                off_track_distance = abs(direction_vector[1] * obstacle_xy[0] - direction_vector[0] * obstacle_xy[1])
-
-                if off_track_distance < min_off_track_distance: # max as fuzzy OR.
-                    min_off_track_distance = off_track_distance
-
-            riskiness = math.exp(-(min_off_track_distance / self.obstacle_influence)**2)
-            return riskiness
-
-        def is_safe(direction):
-            return 1.0 - is_risky(direction)  # Fuzzy negation.
-
-        def is_good(direction):
-            return min(is_safe(direction), is_desired(direction))  # Fuzzy AND.
-
-        return max((is_good(math.radians(direction)), math.radians(direction)) for direction in range(-180, 180, 3) if valid[direction + 180])
-
-class LocalPlannerNumpy:
-    def __init__(self, scan_start=math.radians(-135), scan_end=math.radians(135), direction_adherence=math.radians(90),
-                 max_obstacle_distance=1.5, obstacle_influence=1.2, scan_subsample=1, max_considered_obstacles=None):
-        self.last_scan = None
-        self.scan_start = scan_start
-        self.scan_end = scan_end
-        self.direction_adherence = direction_adherence
-        self.max_obstacle_distance = max_obstacle_distance
-        self.obstacle_influence = obstacle_influence
-
-        self.considered_directions = np.radians(
-                np.linspace(
-                    -180, +180, 360//3, endpoint=False)).reshape((-1, 1))
-        self.considered_directions_cos = np.cos(self.considered_directions)
-        self.considered_directions_sin = np.sin(self.considered_directions)
-        self.scan_subsample = scan_subsample
-
-        # To be filled in later, once we know how many directions per scan we
-        # receive.
-        self.angles = None
-        self.angles_cos = None
-        self.angles_sin = None
-        self.scan = None
-        self.max_considered_obstacles = max_considered_obstacles
-
-    def update(self, scan):
-        self.scan = np.asarray(scan[::self.scan_subsample] if self.scan_subsample > 1 else scan) * 1e-3
-
-        if self.angles is None or len(self.scan) != self.angles.shape[1]:
-            n = len(self.scan)
-            delta = (self.scan_end - self.scan_start) / (n - 1)
-            self.angles = np.linspace(
-                    self.scan_start,
-                    self.scan_end,
-                    n).reshape((1, -1))
-            self.angles_cos = np.cos(self.angles)
-            self.angles_sin = np.sin(self.angles)
-
-    def recommend(self, desired_dir):
-        if self.scan is None:
-            return 0.0, desired_dir
-        NO_MEASUREMENT = 0
-        is_valid = np.logical_and(
-                self.scan != NO_MEASUREMENT,
-                self.scan <= self.max_obstacle_distance)
-        valid_scan = self.scan[is_valid]
-        is_valid = is_valid.reshape((1, -1))
-        acoss = self.angles_cos[is_valid]
-        asins = self.angles_sin[is_valid]
-        x = acoss * valid_scan
-        y = asins * valid_scan
-
-        if x.shape[0] == 0:
-            return 1.0, normalize_angle(desired_dir)
-
-        if self.max_considered_obstacles is not None and x.shape[0] > self.max_considered_obstacles:
-            nth = math.ceil(x.shape[0] / self.max_considered_obstacles)
-            x = x[::nth]
-            y = y[::nth]
-
-        num_considered_directions = self.considered_directions.shape[0]
-        xs = np.broadcast_to(x, (num_considered_directions, x.shape[0]))
-        ys = np.broadcast_to(y, (num_considered_directions, y.shape[0]))
-        dist_from_line = np.abs(xs * self.considered_directions_sin -
-                                ys * self.considered_directions_cos)
-        # Obstacles behind the robot from the perspective of the desired direction do not matter.
-        # The formula below computes cos(angle between the two vectors) * their_norms. Norms are positive, so a negative result implies abs(angle) > 90deg.
-        is_in_front = (xs * self.considered_directions_cos +
-                       ys * self.considered_directions_sin) >= 0
-        # Only valid points that can block the movement count.
-        dist_from_line = np.where(is_in_front, dist_from_line, float('inf'))
-        # In each direction, we only care about the nearest obstacle.
-        min_dist = np.min(dist_from_line, axis=1)
-        risky = np.exp(-(min_dist / self.obstacle_influence)**2)
-        safe = 1.0 - risky  # Fuzzy negation.
-
-        direction_delta = normalize_angle(self.considered_directions - desired_dir)
-        desired = np.exp(-(direction_delta/self.direction_adherence)**2).reshape((-1,))  # Fuzzy equality
-
-        good = np.minimum(safe, desired)  # Fuzzy and.
-        best = np.argmax(good)
-        return good[best], self.considered_directions[best][0]
-
-
-LocalPlanner = LocalPlannerNumpy
-
-# vim: expandtab sw=4 ts=4
diff --git a/subt/odoimuloc.py b/subt/odoimuloc.py
deleted file mode 100644
index f62726fd9..000000000
--- a/subt/odoimuloc.py
+++ /dev/null
@@ -1,47 +0,0 @@
-"""
-  This is revised localization from Virtual to be used in System K2 as backup
-"""
-
-import collections
-import logging
-import math
-from osgar.node import Node
-
-import osgar.lib.quaternion as quaternion
-
-g_logger = logging.getLogger(__name__)
-
-Pose2d = collections.namedtuple("Pose2d", ("x", "y", "heading"))
-
-class Localization(Node):
-    def __init__(self, config, bus):
-        super().__init__(config, bus)
-        # outputs
-        bus.register('pose3d')
-        # inputs: origin, orientation, odom
-        self.xyz = [0, 0, 0]  # 3D position updated using odometry dist and imu orientation, defined when 'origin' received
-        self.orientation = [0, 0, 0, 1]
-        self.last_odom = None
-
-    def on_orientation(self, orientation):
-        self.orientation = orientation
-
-    def on_odom(self, pose2d):
-        x, y, heading = pose2d
-        if self.orientation is None:
-            return
-
-        odom = Pose2d(x / 1000.0, y / 1000.0, math.radians(heading / 100.0))
-        if self.last_odom is not None:
-            dist = math.hypot(odom.x - self.last_odom.x, odom.y - self.last_odom.y)
-            direction = ((odom.x - self.last_odom.x) * math.cos(self.last_odom.heading) +
-                         (odom.y - self.last_odom.y) * math.sin(self.last_odom.heading))
-            if direction < 0:
-                dist = -dist
-        else:
-            dist = 0.0
-
-        self.last_odom = odom
-        dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
-        self.xyz = [a + b for a, b in zip(self.xyz, dist3d)]
-        self.bus.publish('pose3d', [self.xyz, self.orientation])
diff --git a/subt/pull.py b/subt/pull.py
deleted file mode 100644
index 7ab13979e..000000000
--- a/subt/pull.py
+++ /dev/null
@@ -1,51 +0,0 @@
-import contextlib
-
-from threading import Thread
-
-import zmq
-
-from osgar.bus import BusShutdownException
-import osgar.lib.serialize
-
-class Pull:
-
-    def __init__(self, config, bus):
-        bus.register(*config['outputs'])
-        self.is_bind_set = config.get("bind", False)
-        self.endpoint = config.get('endpoint', 'tcp://127.0.0.1:5565')
-        self.timeout = config.get('timeout', 1) # default recv timeout 1s
-        self.thread = Thread(target=self.run)
-        self.thread.name = bus.name
-        self.bus = bus
-
-    def start(self):
-        self.thread.start()
-
-    def join(self, timeout=None):
-        self.thread.join(timeout=timeout)
-
-    def run(self):
-        context = zmq.Context.instance()
-        socket = context.socket(zmq.PULL)
-        # https://stackoverflow.com/questions/7538988/zeromq-how-to-prevent-infinite-wait
-        socket.RCVTIMEO = int(self.timeout * 1000)  # convert to milliseconds
-
-        if self.is_bind_set:
-            socket.LINGER = 100
-            socket.bind(self.endpoint)
-        else:
-            socket.connect(self.endpoint)
-
-        with contextlib.closing(socket):
-            while self.bus.is_alive():
-                try:
-                    channel, raw = socket.recv_multipart()
-                    message = osgar.lib.serialize.deserialize(raw)
-                    self.bus.publish(channel.decode('ascii'), message)
-                except zmq.error.Again:
-                    pass
-
-    def request_stop(self):
-        self.bus.shutdown()
-
-
diff --git a/subt/push.py b/subt/push.py
deleted file mode 100644
index 046574c88..000000000
--- a/subt/push.py
+++ /dev/null
@@ -1,56 +0,0 @@
-import contextlib
-
-from threading import Thread
-
-import zmq
-
-from osgar.bus import BusShutdownException
-from osgar.lib.serialize import serialize
-
-
-class Push:
-
-    def __init__(self, config, bus):
-        bus.register()
-        self.is_bind_set = config.get("bind", False)
-        self.endpoint = config.get('endpoint', 'tcp://127.0.0.1:5566')
-        self.timeout = config.get('timeout', 1) # default send timeout 1s, effective on full queue only
-        self.thread = Thread(target=self.run)
-        self.thread.name = bus.name
-        self.bus = bus
-
-    def start(self):
-        self.thread.start()
-
-    def join(self, timeout=None):
-        self.thread.join(timeout=timeout)
-
-    def run(self):
-        context = zmq.Context.instance()
-        socket = context.socket(zmq.PUSH)
-
-        if self.is_bind_set:
-            socket.setsockopt(zmq.LINGER, 100)  # milliseconds
-            socket.bind(self.endpoint)
-        else:
-            # https://stackoverflow.com/questions/7538988/zeromq-how-to-prevent-infinite-wait
-            socket.SNDTIMEO = int(self.timeout * 1000)  # convert to milliseconds
-            socket.LINGER = 100 #milliseconds
-            socket.connect(self.endpoint)
-
-        try:
-            with contextlib.closing(socket):
-                while True:
-                    dt, channel, data = self.bus.listen()
-                    raw = serialize(data)
-                    socket.send_multipart([bytes(channel, 'ascii'), raw])
-        except zmq.ZMQError as e:
-            if e.errno == zmq.EAGAIN:
-                pass #TODO log timeout
-            else:
-                pass #TODO log other error
-        except BusShutdownException:
-            pass
-
-    def request_stop(self):
-        self.bus.shutdown()

From a191f6efc7b015cf26b300f71dd6d79e7072fdee Mon Sep 17 00:00:00 2001
From: tajgr <levjakub@gmail.com>
Date: Thu, 26 Oct 2023 21:30:22 +0200
Subject: [PATCH 3/7] Move odoimuloc to osgar

---
 osgar/drivers/odoimuloc.py | 47 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 47 insertions(+)
 create mode 100644 osgar/drivers/odoimuloc.py

diff --git a/osgar/drivers/odoimuloc.py b/osgar/drivers/odoimuloc.py
new file mode 100644
index 000000000..f62726fd9
--- /dev/null
+++ b/osgar/drivers/odoimuloc.py
@@ -0,0 +1,47 @@
+"""
+  This is revised localization from Virtual to be used in System K2 as backup
+"""
+
+import collections
+import logging
+import math
+from osgar.node import Node
+
+import osgar.lib.quaternion as quaternion
+
+g_logger = logging.getLogger(__name__)
+
+Pose2d = collections.namedtuple("Pose2d", ("x", "y", "heading"))
+
+class Localization(Node):
+    def __init__(self, config, bus):
+        super().__init__(config, bus)
+        # outputs
+        bus.register('pose3d')
+        # inputs: origin, orientation, odom
+        self.xyz = [0, 0, 0]  # 3D position updated using odometry dist and imu orientation, defined when 'origin' received
+        self.orientation = [0, 0, 0, 1]
+        self.last_odom = None
+
+    def on_orientation(self, orientation):
+        self.orientation = orientation
+
+    def on_odom(self, pose2d):
+        x, y, heading = pose2d
+        if self.orientation is None:
+            return
+
+        odom = Pose2d(x / 1000.0, y / 1000.0, math.radians(heading / 100.0))
+        if self.last_odom is not None:
+            dist = math.hypot(odom.x - self.last_odom.x, odom.y - self.last_odom.y)
+            direction = ((odom.x - self.last_odom.x) * math.cos(self.last_odom.heading) +
+                         (odom.y - self.last_odom.y) * math.sin(self.last_odom.heading))
+            if direction < 0:
+                dist = -dist
+        else:
+            dist = 0.0
+
+        self.last_odom = odom
+        dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
+        self.xyz = [a + b for a, b in zip(self.xyz, dist3d)]
+        self.bus.publish('pose3d', [self.xyz, self.orientation])

From 7e6dac0e6c3d59b1d55f6b44ed277e957bc787d5 Mon Sep 17 00:00:00 2001
From: tajgr <levjakub@gmail.com>
Date: Thu, 26 Oct 2023 21:43:18 +0200
Subject: [PATCH 4/7] Fix local_planner import

---
 osgar/lib/test_local_planner.py | 2 +-
 subt/main.py                    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/osgar/lib/test_local_planner.py b/osgar/lib/test_local_planner.py
index 517aef3d6..a527ba275 100644
--- a/osgar/lib/test_local_planner.py
+++ b/osgar/lib/test_local_planner.py
@@ -1,7 +1,7 @@
 import unittest
 import math
 
-from subt.local_planner import LocalPlanner, LocalPlannerRef, LocalPlannerOpt, LocalPlannerNumpy
+from osgar.lib.local_planner import LocalPlanner, LocalPlannerRef, LocalPlannerOpt, LocalPlannerNumpy
 
 
 class SubTChallengeTest(unittest.TestCase):
diff --git a/subt/main.py b/subt/main.py
index 5f8a8e576..26652e6e4 100644
--- a/subt/main.py
+++ b/subt/main.py
@@ -20,7 +20,7 @@
 from osgar.lib.lidar_pts import equal_scans
 from osgar.lib.loop import LoopDetector
 
-from subt.local_planner import LocalPlanner
+from osgar.lib.local_planner import LocalPlanner
 from subt.trace import Trace, distance3D
 from subt.name_decoder import parse_robot_name
 

From dfb6164cd3132cd782d336894a6584355cc63467 Mon Sep 17 00:00:00 2001
From: tajgr <levjakub@gmail.com>
Date: Thu, 26 Oct 2023 21:53:32 +0200
Subject: [PATCH 5/7] Fix path to odoimuloc.py

---
 config/kloubak2-subt-estop-lora-jetson.json | 2 +-
 config/skiddy-subt.json                     | 2 +-
 osgar/drivers/test_odoimuloc.py             | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/config/kloubak2-subt-estop-lora-jetson.json b/config/kloubak2-subt-estop-lora-jetson.json
index db7805314..83c9994e5 100644
--- a/config/kloubak2-subt-estop-lora-jetson.json
+++ b/config/kloubak2-subt-estop-lora-jetson.json
@@ -160,7 +160,7 @@
           "init": {"port": "/dev/gas_detector", "speed": 115200}
       },
       "localization": {
-	      "driver": "subt.odoimuloc:Localization",
+	      "driver": "osgar.drivers.odoimuloc:Localization",
 	      "in": ["orientation", "odom"],
 	      "out": ["pose3d"],
 	      "init":{}
diff --git a/config/skiddy-subt.json b/config/skiddy-subt.json
index c41171406..5c4175cc7 100644
--- a/config/skiddy-subt.json
+++ b/config/skiddy-subt.json
@@ -44,7 +44,7 @@
           }
       },
       "localization": {
-	      "driver": "subt.odoimuloc:Localization",
+	      "driver": "osgar.drivers.odoimuloc:Localization",
 	      "in": ["orientation", "odom"],
 	      "out": ["pose3d"],
 	      "init":{}
diff --git a/osgar/drivers/test_odoimuloc.py b/osgar/drivers/test_odoimuloc.py
index 6b089f34c..cacb12f10 100644
--- a/osgar/drivers/test_odoimuloc.py
+++ b/osgar/drivers/test_odoimuloc.py
@@ -11,7 +11,7 @@
 from osgar.lib import quaternion
 from osgar.lib.unittest import TestCase
 
-from subt.odoimuloc import Localization
+from osgar.drivers.odoimuloc import Localization
 
 
 class LocalizationTest(TestCase):

From 8d70f0bf81458155b1b46b495ee075160508ed2d Mon Sep 17 00:00:00 2001
From: tajgr <levjakub@gmail.com>
Date: Thu, 26 Oct 2023 22:07:06 +0200
Subject: [PATCH 6/7] Fix path to drivers.pull

---
 _deprecated/subt_graveyard/r2.json          | 2 +-
 config/deedee-subt.json                     | 2 +-
 config/jetson-node-k2-front.json            | 4 ++--
 config/jetson-node-k2-rear.json             | 2 +-
 config/kloubak2-subt-estop-lora-jetson.json | 6 +++---
 config/skiddy-subt-serialloop.json          | 2 +-
 config/skiddy-subt.json                     | 4 ++--
 config/test-pull_zmq.json                   | 2 +-
 subt/config/coro_pam.json                   | 2 +-
 subt/octomap.json                           | 2 +-
 subt/zmq-subt-x2.json                       | 2 +-
 subt/zmq-subt-x4.json                       | 2 +-
 12 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/_deprecated/subt_graveyard/r2.json b/_deprecated/subt_graveyard/r2.json
index 4ffbfdecc..d06c76e3f 100644
--- a/_deprecated/subt_graveyard/r2.json
+++ b/_deprecated/subt_graveyard/r2.json
@@ -96,7 +96,7 @@
           }
       },
       "fromrospy": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
             "outputs": ["acc", "battery_state", "score", "pose3d", "gas_detected",
                         "scan_front", "scan_rear", "rgbd_front:null", "rgbd_rear:null",
diff --git a/config/deedee-subt.json b/config/deedee-subt.json
index 49f4a485d..cdaeb3929 100644
--- a/config/deedee-subt.json
+++ b/config/deedee-subt.json
@@ -190,7 +190,7 @@
         }
       },
       "fromros": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "out": "scan360",
           "init": {
               "outputs": ["scan360"]
diff --git a/config/jetson-node-k2-front.json b/config/jetson-node-k2-front.json
index bc4fec7e1..fcdef5523 100644
--- a/config/jetson-node-k2-front.json
+++ b/config/jetson-node-k2-front.json
@@ -11,7 +11,7 @@
           }
       },
       "from_apu_receiver":{
-        "driver": "subt.pull:Pull",
+        "driver": "osgar.drivers.pull:Pull",
         "init": {
           "bind": true,
           "endpoint": "tcp://*:5558",
@@ -19,7 +19,7 @@
         }
       },
       "from_jetson_rear_receiver":{
-        "driver": "subt.pull:Pull",
+        "driver": "osgar.drivers.pull:Pull",
         "init": {
           "bind": true,
           "endpoint": "tcp://*:5560",
diff --git a/config/jetson-node-k2-rear.json b/config/jetson-node-k2-rear.json
index c991fa85f..dcd64d40e 100644
--- a/config/jetson-node-k2-rear.json
+++ b/config/jetson-node-k2-rear.json
@@ -46,7 +46,7 @@
           }
       },
       "receiver":{
-        "driver": "subt.pull:Pull",
+        "driver": "osgar.drivers.pull:Pull",
         "init": {
           "bind": true,
           "endpoint": "tcp://*:5558",
diff --git a/config/kloubak2-subt-estop-lora-jetson.json b/config/kloubak2-subt-estop-lora-jetson.json
index 83c9994e5..6ce771bf1 100644
--- a/config/kloubak2-subt-estop-lora-jetson.json
+++ b/config/kloubak2-subt-estop-lora-jetson.json
@@ -166,14 +166,14 @@
 	      "init":{}
       },
       "from_jetson_front": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
             "endpoint": "tcp://192.168.0.23:5557",
             "outputs": ["pose3d", "depth", "scan360", "points"]
           }
        },
       "from_jetson_rear": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
             "endpoint": "tcp://192.168.0.24:5557",
             "outputs": ["pose3d", "depth", "localized_artf", "debug_image", "localized_artf_rear", "debug_image_rear"]
@@ -224,7 +224,7 @@
           }
       },
       "fromros": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "out": ["scan360", "points"],
           "init": {
               "endpoint": "tcp://192.168.0.23:5565",
diff --git a/config/skiddy-subt-serialloop.json b/config/skiddy-subt-serialloop.json
index aaee27070..54dc1b922 100644
--- a/config/skiddy-subt-serialloop.json
+++ b/config/skiddy-subt-serialloop.json
@@ -9,7 +9,7 @@
           "init": {"port": "/dev/ttyACM0", "speed": 115200, "timeout": 0.02}
       },
       "fromapp": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "out": ["raw"],
           "init": {
               "endpoint": "tcp://127.0.0.1:5590",
diff --git a/config/skiddy-subt.json b/config/skiddy-subt.json
index 5c4175cc7..0e8f85751 100644
--- a/config/skiddy-subt.json
+++ b/config/skiddy-subt.json
@@ -37,7 +37,7 @@
           }
       },
       "fromserial": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
               "endpoint": "tcp://127.0.0.1:5591",
               "outputs": ["raw"]
@@ -245,7 +245,7 @@
         }
       },
       "fromros": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "out": ["scan360", "points"],
           "init": {
               "outputs": ["scan360", "points"]
diff --git a/config/test-pull_zmq.json b/config/test-pull_zmq.json
index 049aca1bd..ea92ebfa0 100644
--- a/config/test-pull_zmq.json
+++ b/config/test-pull_zmq.json
@@ -3,7 +3,7 @@
   "robot": {
     "modules": {
       "pull": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
             "endpoint": "tcp://192.168.0.23:5557",
             "outputs": ["localized_artf", "debug_image"]
diff --git a/subt/config/coro_pam.json b/subt/config/coro_pam.json
index 14aada7bc..28fcb5dfe 100644
--- a/subt/config/coro_pam.json
+++ b/subt/config/coro_pam.json
@@ -118,7 +118,7 @@
           }
       },
       "fromrospy": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
               "outputs": ["acc", "top_scan", "bottom_scan", "pose3d", "battery_state", "score", "gas_detected",
                           "rgbd_front:null", "rgbd_left:null", "rgbd_right:null",
diff --git a/subt/octomap.json b/subt/octomap.json
index 4d0469918..268bb9008 100644
--- a/subt/octomap.json
+++ b/subt/octomap.json
@@ -3,7 +3,7 @@
   "robot": {
     "modules": {
       "fromrospy": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
             "outputs": ["rot", "acc", "orientation", "battery_state", "score", "pose3d",
                         "image_front", "image_rear", "scan_front", "scan_rear", "depth_front:null", "depth_rear:null",
diff --git a/subt/zmq-subt-x2.json b/subt/zmq-subt-x2.json
index 56eb54eed..f6fb5de7a 100644
--- a/subt/zmq-subt-x2.json
+++ b/subt/zmq-subt-x2.json
@@ -81,7 +81,7 @@
           }
       },
       "fromrospy": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
           "init": {
             "outputs": ["acc", "battery_state", "score", "pose3d", "gas_detected", "joint_angle",
                         "scan_front", "scan_rear", "scan360", "rgbd_front:null", "rgbd_rear:null",
diff --git a/subt/zmq-subt-x4.json b/subt/zmq-subt-x4.json
index 2f95d0591..b44517aea 100644
--- a/subt/zmq-subt-x4.json
+++ b/subt/zmq-subt-x4.json
@@ -117,7 +117,7 @@
           }
       },
       "fromrospy": {
-          "driver": "subt.pull:Pull",
+          "driver": "osgar.drivers.pull:Pull",
 	      "init": {
               "outputs": ["acc", "top_scan", "bottom_scan", "pose3d", "battery_state", "score", "gas_detected",
                           "rgbd_front:null", "air_pressure", "scan360", "octomap:gz", "robot_name", "slopes"]

From 203f076538e00224c66c46337635fe1e2ad93eac Mon Sep 17 00:00:00 2001
From: tajgr <levjakub@gmail.com>
Date: Thu, 26 Oct 2023 22:16:13 +0200
Subject: [PATCH 7/7] Fix path for drivers/push

---
 _deprecated/subt_graveyard/r2.json          | 2 +-
 config/deedee-subt.json                     | 2 +-
 config/jetson-node-k2-front.json            | 4 ++--
 config/jetson-node-k2-rear.json             | 4 ++--
 config/kloubak2-subt-estop-lora-jetson.json | 4 ++--
 config/skiddy-subt-serialloop.json          | 2 +-
 config/skiddy-subt.json                     | 4 ++--
 subt/config/coro_pam.json                   | 2 +-
 subt/zmq-subt-x2.json                       | 2 +-
 subt/zmq-subt-x4.json                       | 2 +-
 10 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/_deprecated/subt_graveyard/r2.json b/_deprecated/subt_graveyard/r2.json
index d06c76e3f..3f03d9426 100644
--- a/_deprecated/subt_graveyard/r2.json
+++ b/_deprecated/subt_graveyard/r2.json
@@ -104,7 +104,7 @@
           }
       },
       "torospy": {
-          "driver": "subt.push:Push"
+          "driver": "osgar.drivers.push:Push"
       },
       "logimage": {
           "driver": "subt.image_extractor:ImageExtractor",
diff --git a/config/deedee-subt.json b/config/deedee-subt.json
index cdaeb3929..2e674e0dc 100644
--- a/config/deedee-subt.json
+++ b/config/deedee-subt.json
@@ -197,7 +197,7 @@
           }
       },
       "toros": {
-          "driver": "subt.push:Push"
+          "driver": "osgar.drivers.push:Push"
       },
       "sec_timer": {
         "driver": "timer",
diff --git a/config/jetson-node-k2-front.json b/config/jetson-node-k2-front.json
index fcdef5523..0289d41f2 100644
--- a/config/jetson-node-k2-front.json
+++ b/config/jetson-node-k2-front.json
@@ -4,7 +4,7 @@
     "modules":
     {
       "transmitter": {
-          "driver": "subt.push:Push",
+          "driver": "osgar.drivers.push:Push",
           "init": {
             "bind": true,
             "endpoint": "tcp://*:5557"
@@ -92,7 +92,7 @@
         }
       },
       "toros": {
-          "driver": "subt.push:Push"
+          "driver": "osgar.drivers.push:Push"
       }
     },
     "links": [
diff --git a/config/jetson-node-k2-rear.json b/config/jetson-node-k2-rear.json
index dcd64d40e..d9ccb60ea 100644
--- a/config/jetson-node-k2-rear.json
+++ b/config/jetson-node-k2-rear.json
@@ -39,7 +39,7 @@
         }
       },
       "transmitter": {
-          "driver": "subt.push:Push",
+          "driver": "osgar.drivers.push:Push",
           "init": {
             "bind": true,
             "endpoint": "tcp://*:5557"
@@ -69,7 +69,7 @@
         }
       },
       "to_front_jetson": {
-        "driver": "subt.push:Push",
+        "driver": "osgar.drivers.push:Push",
           "init": {
             "endpoint": "tcp://192.168.0.23:5560"
           }
diff --git a/config/kloubak2-subt-estop-lora-jetson.json b/config/kloubak2-subt-estop-lora-jetson.json
index 6ce771bf1..fb7d9f5c7 100644
--- a/config/kloubak2-subt-estop-lora-jetson.json
+++ b/config/kloubak2-subt-estop-lora-jetson.json
@@ -180,13 +180,13 @@
           }
        },
       "to_jetson_rear": {
-          "driver": "subt.push:Push",
+          "driver": "osgar.drivers.push:Push",
           "init": {
             "endpoint": "tcp://192.168.0.24:5558"
           }
       },
        "to_jetson_front": {
-          "driver": "subt.push:Push",
+          "driver": "osgar.drivers.push:Push",
           "init": {
             "endpoint": "tcp://192.168.0.23:5558"
           }
diff --git a/config/skiddy-subt-serialloop.json b/config/skiddy-subt-serialloop.json
index 54dc1b922..13041aad8 100644
--- a/config/skiddy-subt-serialloop.json
+++ b/config/skiddy-subt-serialloop.json
@@ -18,7 +18,7 @@
           }
       },
       "toapp": {
-          "driver": "subt.push:Push",
+          "driver": "osgar.drivers.push:Push",
           "init": {
               "endpoint": "tcp://127.0.0.1:5591",
               "bind": true
diff --git a/config/skiddy-subt.json b/config/skiddy-subt.json
index 0e8f85751..25620df53 100644
--- a/config/skiddy-subt.json
+++ b/config/skiddy-subt.json
@@ -31,7 +31,7 @@
           "init": {}
       },
       "toserial": {
-          "driver": "subt.push:Push",
+          "driver": "osgar.drivers.push:Push",
           "init": {
               "endpoint": "tcp://127.0.0.1:5590"
           }
@@ -260,7 +260,7 @@
         }
       },
       "toros": {
-          "driver": "subt.push:Push"
+          "driver": "osgar.drivers.push:Push"
       },
       "sec_timer": {
         "driver": "timer",
diff --git a/subt/config/coro_pam.json b/subt/config/coro_pam.json
index 28fcb5dfe..880aed7c6 100644
--- a/subt/config/coro_pam.json
+++ b/subt/config/coro_pam.json
@@ -126,7 +126,7 @@
           }
       },
       "torospy": {
-          "driver": "subt.push:Push"
+          "driver": "osgar.drivers.push:Push"
       },
       "logimage": {
           "driver": "subt.image_extractor:ImageExtractor",
diff --git a/subt/zmq-subt-x2.json b/subt/zmq-subt-x2.json
index f6fb5de7a..c7abf5af0 100644
--- a/subt/zmq-subt-x2.json
+++ b/subt/zmq-subt-x2.json
@@ -89,7 +89,7 @@
           }
       },
       "torospy": {
-          "driver": "subt.push:Push"
+          "driver": "osgar.drivers.push:Push"
       },
       "twister": {
           "driver": "subt.twistwrap:TwistWrap"
diff --git a/subt/zmq-subt-x4.json b/subt/zmq-subt-x4.json
index b44517aea..70768b439 100644
--- a/subt/zmq-subt-x4.json
+++ b/subt/zmq-subt-x4.json
@@ -124,7 +124,7 @@
           }
       },
       "torospy": {
-          "driver": "subt.push:Push"
+          "driver": "osgar.drivers.push:Push"
       },
       "logimage": {
           "driver": "subt.image_extractor:ImageExtractor",