fixed bug and added test

tests/test_trajectories.py

@@ -0,0 +1,22 @@
+def test_imports():
+    from trajectories import TrajectoryFactory
+
+def test_square_trajectory():
+    from trajectories import TrajectoryFactory 
+    from trajectories.trajectories import SquareLinearTrajectory
+    import numpy as np
+    trajectory: SquareLinearTrajectory = TrajectoryFactory.get_linear_square_trajectory()
+    p0 = trajectory.get_waypoint(0) 
+    p1 = trajectory.get_waypoint(0.25)
+    p2 = trajectory.get_waypoint(0.5)
+    p3 = trajectory.get_waypoint(0.75)
+
+    # assert corner points
+    assert((p0.coordinate == trajectory.corner_points[0]).all())
+    assert((p1.coordinate == trajectory.corner_points[1]).all())
+    assert((p2.coordinate == trajectory.corner_points[2]).all())
+    assert((p3.coordinate == trajectory.corner_points[3]).all())
+
+    # assert interpolation
+    assert(trajectory.get_waypoint(0.1).coordinate == np.asarray([0.4, 0., 1.], dtype=np.float32)).all()
+

trajectories/trajectories.py

@@ -44,8 +44,8 @@ def get_waypoint(self, time: float):
             # in-between two points, linear interpolation
             cur_corner = math.floor(time)
             upcomimg_corner = math.ceil(time)
-            diff = upcomimg_corner - cur_corner
-            target_pos = cur_corner + (time - int(time)) * (diff)
+            diff = self.corner_points[upcomimg_corner] - self.corner_points[cur_corner]
+            target_pos = self.corner_points[cur_corner] + (time - int(time)) * (diff)
 
         target_wp = Waypoint(
             coordinate=target_pos,

trajectories/trajectory_factory.py

@@ -6,7 +6,7 @@ class TrajectoryFactory:
     """
 
     @classmethod
-    def get_linear_square_trajectory(cls, square_scale: float, time_scale: float) -> Trajectory:
+    def get_linear_square_trajectory(cls, square_scale: float=1, time_scale: float=1) -> Trajectory:
         return SquareLinearTrajectory(
             square_scale, time_scale
         )