data

DroneController/TrajFollower.py

@@ -165,8 +165,14 @@ def sendTrajectory(self):
         self.VMAX = 0.0
 
 
-        # self.traj = np.loadtxt(self.traj_path, delimiter=",")
+        # self.traj_path = "trajs/traj_2obs_vicon.csv"
+        # self.traj_path = 'trajs/traj_L_vicon.csv'
 
+        # # # comment out the below 3 lines for different traj
+        # self.traj = np.loadtxt(self.traj_path, delimiter=",")[:, :2]
+        # # self.traj = np.flipud(self.traj)
+        # # self.traj = np.fliplr(self.traj)
+
         # compute trajectory 
         xx, yy = generate_spiral_eight(num_points=self.num_points)
         self.traj = np.column_stack((xx, yy))

DroneController/ViconClient.py

@@ -40,6 +40,8 @@ def pose_callback(self):
         latency, frameno, position = self.tracker.get_position(self.obj_name)
         if position != []:
             xyz_position = position[0][2:5] # get x,y,z only
+            # xyz_position[0] *= -1 
+            # xyz_position[1] *= -1 
             orientation = position[0][7] # get rotation around z axis
             data = {"position": np.array(xyz_position) / 1000.0, "orientation" : orientation}
             self.pose_signal.emit(data)

run.sh

@@ -1,5 +1,5 @@
 #!/usr/bin/bash
 
 PYTHON="/home/airlab/anaconda3/envs/droneController/bin/python"
-
+cd "/home/airlab/PyDev/droneController"
 $PYTHON workspace.py 

trajs/.~lock.traj_2obs_vicon.csv#

@@ -0,0 +1 @@
+,airlab,airlab-HP-EliteDesk-800-G3-TWR,12.08.2024 13:38,file:///home/airlab/.config/libreoffice/4;

trajs/.~lock.traj_L_vicon.csv#

@@ -0,0 +1 @@
+,airlab,airlab-HP-EliteDesk-800-G3-TWR,12.08.2024 14:02,file:///home/airlab/.config/libreoffice/4;

trajs/interpolate.py

@@ -0,0 +1,53 @@
+import numpy as np
+from matplotlib import pyplot as plt 
+
+def interpolate_2d(p1, p2, distance):
+    """
+    Linearly interpolate between two 2D points based on distance.
+    
+    Parameters:
+    p1 (tuple): First point (x1, y1)
+    p2 (tuple): Second point (x2, y2)
+    distance (float): Desired distance from p1 towards p2
+    
+    Returns:
+    tuple: Interpolated point (x, y)
+    """
+    x1, y1 = p1
+    x2, y2 = p2
+
+    # Calculate total distance between points
+    total_distance = np.sqrt((x2 - x1)**2 + (y2 - y1)**2)
+
+    # Calculate interpolation factor
+    t = distance / total_distance
+
+    # Ensure t is between 0 and 1
+    t = max(0, min(1, t))
+
+    # Interpolate x and y coordinates
+    x = x1 + t * (x2 - x1)
+    y = y1 + t * (y2 - y1)
+
+    return (x, y)
+
+
+if __name__ == "__main__":
+    L1 = np.array([[1540.399, 1320.082], [1601.358, -177.795], [-970.348, -154.368]]) / 1000.0
+    L2 = np.array([[-970.348, -154.368], [3187.236, -205.191], [2198.592, -1947.821]]) / 1000.0
+
+
+    # path1 = np.array([interpolate_2d(p1, p2, 0.15) for p1, p2 in L1])
+    # path2 = np.array([interpolate_2d(p1, p2, 0.15) for p1, p2 in L1])
+
+    for i, p in enumerate(L1):
+        if i == 0:
+            continue
+        points = interpolate_2d(L1[i-1], L1[i], 0.15)
+        print(points)
+
+    # plt.plot(path1[:, 0], path1[:, 1])
+    # plt.show()
+
+
+

trajs/traj_2obs_vicon.csv

@@ -0,0 +1,54 @@
+-8.299999999999999600e-01,-1.836999999999999966e+00,1.000000000000000000e+00
+-7.450009766038754311e-01,-1.730148176934997784e+00,9.666666666666666741e-01
+-6.732094693608344782e-01,-1.617727069642602267e+00,9.333333333333333481e-01
+-6.127077209589250062e-01,-1.500718453063208724e+00,9.000000000000000222e-01
+-5.614818009297389834e-01,-1.379867714941392709e+00,8.666666666666666963e-01
+-5.174414402223255216e-01,-1.255751744018448246e+00,8.333333333333333703e-01
+-4.784349769579034906e-01,-1.128849487975007504e+00,8.000000000000000444e-01
+-4.422629474196489996e-01,-9.996145186081809575e-01,7.666666666666667185e-01
+-4.066939460413602214e-01,-8.685488864882431326e-01,7.333333333333333925e-01
+-3.694864109755313653e-01,-7.362772529944685562e-01,7.000000000000000666e-01
+-3.284199353272029276e-01,-6.036197553983935205e-01,6.666666666666667407e-01
+-2.813395068693251844e-01,-4.716613014086229461e-01,6.333333333333334147e-01
+-2.262156722140321874e-01,-3.418140293991329681e-01,6.000000000000000888e-01
+-1.612229224834268171e-01,-2.158685578188107301e-01,5.666666666666667629e-01
+-8.483751657306651262e-02,-9.602846083151206358e-02,5.333333333333334370e-01
+4.045592306325729304e-03,1.507873557164422784e-02,5.000000000000001110e-01
+1.059791516409798323e-01,1.144216791395017280e-01,4.666666666666667851e-01
+1.950075113927089432e-01,1.912763973608800350e-01,5.083333333333334147e-01
+2.214872799386580215e-01,2.598686164083160444e-01,5.750000000000000666e-01
+2.125925962902268096e-01,3.195713400922356451e-01,6.166666666666666963e-01
+2.079693593430856635e-01,3.832199343962959803e-01,5.583333333333333481e-01
+2.040434112067165684e-01,4.518545762024127610e-01,5.666666666666666519e-01
+2.014534969122979968e-01,5.237532988982975812e-01,5.083333333333333037e-01
+2.060424277862622500e-01,5.868621150830488631e-01,4.833333333333332815e-01
+2.144310017189269835e-01,6.584775773020118406e-01,4.666666666666666186e-01
+2.216722900609196523e-01,7.321555176058034409e-01,4.416666666666665964e-01
+2.320972683112977886e-01,8.059800531062989437e-01,4.249999999999999334e-01
+2.374767479595125685e-01,8.825850834668040878e-01,3.666666666666665853e-01
+2.487750559944096884e-01,9.499331493483137434e-01,3.416666666666665630e-01
+2.762099574344731301e-01,9.762973842541801117e-01,3.333333333333332038e-01
+2.899711155728670331e-01,1.042220659602041133e+00,3.083333333333331816e-01
+2.540660354501778073e-01,1.061180022831516467e+00,3.499999999999998668e-01
+2.722501877571185669e-01,1.091569003603025001e+00,3.416666666666665075e-01
+2.863589916674630920e-01,1.122730324675994096e+00,3.333333333333331483e-01
+2.961191975043004354e-01,1.154953384026603302e+00,3.249999999999997891e-01
+3.012508798827253087e-01,1.188472780088129399e+00,3.166666666666664298e-01
+3.014610660199024683e-01,1.223463101766075978e+00,3.083333333333330706e-01
+3.250509598799229960e-01,1.240152520775734013e+00,2.916666666666664076e-01
+2.599444749552340950e-01,1.268564788422566370e+00,3.333333333333330373e-01
+2.328350376904378960e-01,1.242273510632923683e+00,3.999999999999996891e-01
+1.934009278531922393e-01,1.218401465658118576e+00,4.833333333333330040e-01
+1.890537612914772458e-01,1.151095373014281664e+00,5.666666666666663188e-01
+1.989701917003015574e-01,1.103471393157673175e+00,6.166666666666663632e-01
+2.051630553039033023e-01,1.158022021148545466e+00,6.999999999999997335e-01
+2.038196686182897621e-01,1.224286329930003303e+00,7.249999999999997558e-01
+1.946902384297918565e-01,1.262814808570267289e+00,8.166666666666664298e-01
+2.053725904692363102e-01,1.302579167104369162e+00,8.916666666666663854e-01
+1.942874641113844780e-01,1.306652685224906740e+00,9.833333333333330595e-01
+2.016833735190618759e-01,1.402957447555637005e+00,1.041666666666666297e+00
+1.924967843317423877e-01,1.504247403710541331e+00,1.108333333333332948e+00
+2.121702828190488166e-01,1.601598826130484987e+00,1.174999999999999600e+00
+3.081163865777842292e-01,1.690351056745890368e+00,1.216666666666666341e+00
+4.439997678723078822e-01,1.783415148566596997e+00,1.183333333333332904e+00
+6.050058529824089426e-01,1.800442405932133116e+00,1.249999999999999556e+00

trajs/traj_L_vicon.csv

@@ -0,0 +1,54 @@
+1.80044240593213,0.605005852982409,1
+1.7834151485666,0.443999767872308,1
+1.69035105674589,0.308116386577784,1
+1.60159882613049,0.212170282819049,1
+1.50424740371054,0.192496784331742,1
+1.40295744755564,0.201683373519062,1
+1.30665268522491,0.201683373519,1
+1.30257916710437,0.201683373519062,1
+1.26281480857027,0.201683373519062,1
+1.22428632993,0.201683373519062,1
+1.15802202114855,0.201683373519062,1
+1.10347139315767,0.201683373519062,1
+1.15109537301428,0.201683373519062,1
+1.21840146565812,0.201683373519062,1
+1.24227351063292,0.201683373519062,1
+1.26856478842257,0.201683373519062,1
+1.24015252077573,0.201683373519062,1
+1.22346310176608,0.201683373519062,1
+1.18847278008813,0.201683373519062,1
+1.1549533840266,0.201683373519062,1
+1.12273032467599,0.201683373519062,1
+1.09156900360303,0.201683373519062,1
+1.06118002283152,0.201683373519062,1
+1.04222065960204,0.201683373519062,1
+0.97629738425418,0.201683373519062,1
+0.949933149348314,0.201683373519062,1
+0.882585083466804,0.201683373519062,1
+0.805980053106299,0.201683373519062,1
+0.732155517605803,0.201683373519062,1
+0.658477577302012,0.201683373519062,1
+0.586862115083049,0.201683373519062,1
+0.523753298898298,0.201683373519062,1
+0.451854576202413,0.201683373519062,1
+0.383219934396296,0.201683373519062,1
+0.319571340092236,0.201683373519062,1
+0.259868616408316,0.201683373519062,1
+0.19127639736088,0.201683373519062,1
+0.114421679139502,0.201683373519062,1
+0.0150787355716442,0.201683373519062,1
+-0.0960284608315121,0.201683373519062,1
+-0.215868557818811,0.201683373519062,1
+-0.341814029399133,0.201683373519062,1
+-0.471661301408623,0.201683373519062,1
+-0.603619755398394,0.201683373519062,1
+-0.736277252994469,0.201683373519062,1
+-0.868548886488243,0.201683373519062,1
+-0.999614518608181,0.201683373519062,1
+-1.12884948797501,0.201683373519062,1
+-1.25575174401845,0.201683373519062,1
+-1.37986771494139,0.201683373519062,1
+-1.50071845306321,0.201683373519062,1
+-1.6177270696426,0.201683373519062,1
+-1.730148176935,0.201683373519062,1
+-1.837,0.201683373519062,1