Merge pull request #52 from ucla-mobility/feature/unitest_kf_ekf

Feature/unitest kf ekf

.travis.yml

@@ -7,7 +7,7 @@ python:
 
 install:
   - pip install --upgrade pip
-  - pip install -r requirements.txt
+  - pip install -r requirements_ci.txt
   - pip install pytest
   - pip install pytest-cov
   - pip install coveralls
@@ -16,12 +16,12 @@ install:
 script:
    pytest --cov=test/
 
-#after_success:
-#  - coveralls
+after_success:
+  - coveralls
 
 notifications:
   email:
     recipients:
       - derrickxu1994@gmail.com
-  on_success: change # default: change
+  on_success: never # default: change
   on_failure: always # default: always

opencda/core/plan/drive_profile_plotting.py

@@ -102,49 +102,42 @@ def draw_dist_gap_profile_singel_plot(gap_list):
     fig.set_size_inches(11, 5)
 
 
-def draw_sub_plot(velocity_list, acceleration_list, time_gap_list, distance_gap_list):
+def draw_sub_plot(velocity_list, acceleration_list, time_gap_list, distance_gap_list, ttc_list):
     """
     This is a specific function that draws 4 in 1 images for trajectory following task
     :param velocity_list:
     :param distance_gap_list:
     :param time_gap_list:
     :param acceleration_list:
+    :param ttc_list:
     :return:
     """
-    fig = plt.figure(figsize=[2200, 1000])
-    plt.subplot(411)
+    fig = plt.figure()
+    plt.subplot(511)
     draw_velocity_profile_single_plot(velocity_list)
 
-    plt.subplot(412)
+    plt.subplot(512)
     draw_acceleration_profile_single_plot(acceleration_list)
 
-    plt.subplot(413)
+    plt.subplot(513)
     draw_time_gap_profile_singel_plot(time_gap_list)
 
-    plt.subplot(414)
+    plt.subplot(514)
+    draw_dist_gap_profile_singel_plot(distance_gap_list)
+
+    plt.subplot(515)
     draw_dist_gap_profile_singel_plot(distance_gap_list)
 
     label = []
     for i in range(1, len(velocity_list)+1):
         label.append('Leading Vehicle, id: %d' % int(i - 1) if i == 1 else 'Following Vehicle, id: %d' % int(i - 1))
 
     fig.legend(label, loc='upper right')
-    plt.get_current_fig_manager().window.showMaximized()
-
-    plt.show()
-
 
-def dump_data(data):
-    """
-    Dump data to json file
-    :param data: dictionary containing all stats
-    :return:
-    """
-    with open("platooning.json", "w") as outfile:
-        json.dump(data, outfile)
+    return fig
 
 
 if __name__ == '__main__':
     velocity_list = [[23, 25, 25, 44, 66], [44, 55, 25, 22, 33]]
-    ids = [23, 45]
-    draw_sub_plot(velocity_list, velocity_list, velocity_list, velocity_list)
+    fig = draw_sub_plot(velocity_list, velocity_list, velocity_list, velocity_list, velocity_list)
+    plt.show()

opencda/core/plan/planer_debug_helper.py

@@ -45,7 +45,6 @@ def update(self, ego_speed, ttc):
             if len(self.speed_list[0]) <= 1:
                 self.acc_list[0].append(0)
             else:
-                # todo: time-step hardcoded
                 self.acc_list[0].append((self.speed_list[0][-1] - self.speed_list[0][-2]) / 0.05)
             self.ttc_list[0].append(ttc)
 

opencda/core/sensing/localization/localization_debug_helper.py

@@ -100,7 +100,10 @@ def run_step(self, gnss_x, gnss_y, gnss_yaw, gnss_spd,
 
         if self.show_animation:
             # call backend setting here to solve the conflict between cv2 pyqt5 and pyplot qtagg
-            matplotlib.use('TkAgg')
+            try:
+                matplotlib.use('TkAgg')
+            except ImportError:
+                pass
             xEst = np.array([filter_x, filter_y]).reshape(2, 1)
             zTrue = np.array([gt_x, gt_y]).reshape(2, 1)
             z = np.array([gnss_x, gnss_y]).reshape(2, 1)

opencda/customize/ml_libs/ml_manager.py

@@ -9,10 +9,9 @@
 # License: MIT
 
 import cv2
+import numpy as np
 import torch
 
-from opencda.core.sensing.perception.obstacle_vehicle import is_vehicle_cococlass
-
 
 class MLManager(object):
     """
@@ -59,3 +58,16 @@ def draw_2d_box(self, result, rgb_image, index):
 
         return rgb_image
 
+
+def is_vehicle_cococlass(label):
+    """
+    Check whether the label belongs to the vehicle class according to coco dataset.
+    Args:
+        label(int):
+
+    Returns:
+        is_vehicle: bool
+            whether this label belongs to the vehicle class
+    """
+    vehicle_class_array = np.array([2, 3, 4, 6, 8], dtype=np.int)
+    return True if 0 in (label - vehicle_class_array) else False

requirements_ci.txt

@@ -0,0 +1,17 @@
+matplotlib==3.4.2
+networkx==2.5.1
+numpy
+pillow==8.2.0
+pyparsing==2.4.7
+six==1.16.0
+open3d
+opencv-python==4.5.2.52
+pandas
+pygame
+scikit-learn==0.24.2
+scipy==1.6.3
+seaborn
+--find-links https://download.pytorch.org/whl/torch_stable.html
+torch==1.9.0+cpu
+torchvision==0.10.0+cpu
+tqdm>=4.41.0

test/test_drive_profile_plotting.py

@@ -0,0 +1,29 @@
+# -*- coding: utf-8 -*-
+"""
+Unit test for
+"""
+# Author: Runsheng Xu <rxx3386@ucla.edu>
+# License: MIT
+
+import os
+import sys
+import unittest
+
+import numpy as np
+
+# temporary solution for relative imports in case opencda is not installed
+# if opencda is installed, no need to use the following line
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '.')))
+
+import mocked_carla as mcarla
+
+from opencda.core.plan.drive_profile_plotting import *
+
+
+class TestDriveProfile(unittest.TestCase):
+    def setUp(self):
+        self.mock_list = [[23, 25, 25, 44, 66], [44, 55, 25, 22, 33]]
+
+    def test_sub_plot(self):
+        assert draw_sub_plot(self.mock_list, self.mock_list, self.mock_list, self.mock_list, self.mock_list)

test/test_ekf.py

@@ -0,0 +1,49 @@
+# -*- coding: utf-8 -*-
+"""
+Unit test for Extented Kalman Filter.
+"""
+# Author: Runsheng Xu <rxx3386@ucla.edu>
+# License: MIT
+
+import os
+import sys
+import unittest
+
+import numpy as np
+
+# temporary solution for relative imports in case opencda is not installed
+# if opencda is installed, no need to use the following line
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '.')))
+
+import mocked_carla as mcarla
+from opencda.core.sensing.localization.extented_kalman_filter import ExtentedKalmanFilter
+
+
+class testKalmanFilter(unittest.TestCase):
+    def setUp(self):
+        self.dt = 0.25
+        self.kf = ExtentedKalmanFilter(self.dt)
+        self.kf.run_step_init(10, 10, 90, 20)
+
+    def test_parameters(self):
+        assert (hasattr(self.kf, 'Q') and
+                self.kf.Q.shape == (4, 4))
+        assert (hasattr(self.kf, 'R') and
+                self.kf.R.shape == (3, 3))
+        assert (hasattr(self.kf, 'time_step') and
+                self.kf.time_step == self.dt)
+        assert (hasattr(self.kf, 'xEst') and
+                self.kf.xEst.shape == (4, 1))
+        assert (hasattr(self.kf, 'PEst') and
+                self.kf.PEst.shape == (4, 4))
+
+    def test_run_step(self):
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[0], float)
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[1], float)
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[2], float)
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[3], float)
+
+
+if __name__ == '__main__':
+    unittest.main()

test/test_kf.py

@@ -0,0 +1,55 @@
+# -*- coding: utf-8 -*-
+"""
+Unit test for Kalman Filter
+"""
+# Author: Runsheng Xu <rxx3386@ucla.edu>
+# License: MIT
+
+import os
+import sys
+import unittest
+
+import numpy as np
+
+# temporary solution for relative imports in case opencda is not installed
+# if opencda is installed, no need to use the following line
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '.')))
+
+import mocked_carla as mcarla
+from opencda.core.sensing.localization.kalman_filter import KalmanFilter
+from opencda.core.sensing.localization.coordinate_transform import geo_to_transform
+
+
+class testKalmanFilter(unittest.TestCase):
+    def setUp(self):
+        self.dt = 0.25
+        self.kf = KalmanFilter(self.dt)
+        self.kf.run_step_init(10, 10, 90, 20)
+
+    def test_parameters(self):
+        assert (hasattr(self.kf, 'Q') and
+                self.kf.Q.shape == (4, 4))
+        assert (hasattr(self.kf, 'R') and
+                self.kf.R.shape == (3, 3))
+        assert (hasattr(self.kf, 'time_step') and
+                self.kf.time_step == self.dt)
+        assert (hasattr(self.kf, 'xEst') and
+                self.kf.xEst.shape == (4, 1))
+        assert (hasattr(self.kf, 'PEst') and
+                self.kf.PEst.shape == (4, 4))
+
+    def test_run_step(self):
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[0], float)
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[1], float)
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[2], float)
+        assert isinstance(self.kf.run_step(10, 10, 10, 10, 3)[3], float)
+
+    def test_geo_to_transform(self):
+        assert isinstance(geo_to_transform(100, 70, 10, 10, 10, 10)[0], float)
+        assert isinstance(geo_to_transform(100, 70, 10, 10, 10, 10)[1], float)
+        assert isinstance(geo_to_transform(100, 70, 10.0, 10, 10, 10.0)[2], float)
+
+
+if __name__ == '__main__':
+    unittest.main()

test/test_localization_debug_helper.py

@@ -0,0 +1,75 @@
+# -*- coding: utf-8 -*-
+"""
+Unit test for Localization DebugHelper.
+"""
+# Author: Runsheng Xu <rxx3386@ucla.edu>
+# License: MIT
+
+import os
+import sys
+import unittest
+
+import numpy as np
+
+# temporary solution for relative imports in case opencda is not installed
+# if opencda is installed, no need to use the following line
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '.')))
+
+import mocked_carla as mcarla
+from opencda.core.sensing.localization.localization_debug_helper import LocDebugHelper
+
+
+class TestLocDebugHelper(unittest.TestCase):
+    def setUp(self):
+        config_yaml = {'show_animation': True,
+                       'x_scale': 10.0,
+                       'y_scale': 10.0}
+        self.actor_id = 10
+        self.debug_heloer = LocDebugHelper(config_yaml=config_yaml, actor_id=self.actor_id)
+
+    def test_parameters(self):
+        assert isinstance(self.debug_heloer.show_animation, bool)
+        assert isinstance(self.debug_heloer.x_scale, float)
+        assert isinstance(self.debug_heloer.y_scale, float)
+
+        assert isinstance(self.debug_heloer.gnss_x, list)
+        assert isinstance(self.debug_heloer.gnss_y, list)
+        assert isinstance(self.debug_heloer.gnss_yaw, list)
+        assert isinstance(self.debug_heloer.gnss_spd, list)
+
+        assert isinstance(self.debug_heloer.filter_x, list)
+        assert isinstance(self.debug_heloer.filter_y, list)
+        assert isinstance(self.debug_heloer.filter_yaw, list)
+        assert isinstance(self.debug_heloer.filter_spd, list)
+
+        assert isinstance(self.debug_heloer.gt_x, list)
+        assert isinstance(self.debug_heloer.gt_y, list)
+        assert isinstance(self.debug_heloer.gt_yaw, list)
+        assert isinstance(self.debug_heloer.gt_spd, list)
+
+        assert self.debug_heloer.hxEst.shape == (2, 1)
+        assert self.debug_heloer.hTrue.shape == (2, 1)
+        assert self.debug_heloer.hz.shape == (2, 1)
+
+        assert self.debug_heloer.actor_id == self.actor_id
+
+    def test_run_step(self):
+        self.debug_heloer.run_step(10.0, 10.0, 10.0, 20.0,
+                                   10.4, 10.4, 10.4, 20.4,
+                                   10.3, 10.3, 10.3, 20.3)
+
+        assert len(self.debug_heloer.gnss_x) == 1
+        assert len(self.debug_heloer.filter_x) == 1
+        assert len(self.debug_heloer.gt_x) == 1
+        assert self.debug_heloer.hTrue.shape[1] == 2
+        assert self.debug_heloer.hxEst.shape[1] == 2
+        assert self.debug_heloer.hz.shape[1] == 2
+
+    def test_evaluate(self):
+        self.debug_heloer.run_step(10.0, 10.0, 10.0, 20.0,
+                                   10.4, 10.4, 10.4, 20.4,
+                                   10.3, 10.3, 10.3, 20.3)
+        assert self.debug_heloer.evaluate()[0]
+        assert isinstance(self.debug_heloer.evaluate()[1], str)
+