kinematics evlauation added

opencda.py

@@ -34,6 +34,7 @@ def arg_parse():
 
 def main():
  opt = arg_parse()
+ print("OpenCDA Version: %s" % __version__)
 
  try:
  testing_scenario = importlib.import_module("opencda.scenario_testing.%s" % opt.test_scenario)
@@ -54,4 +55,4 @@ def main():
  try:
  main()
  except KeyboardInterrupt:
- print(' - Exited by user.')
+ print(' - Exited by user.')

opencda/core/plan/behavior_agent.py

@@ -14,11 +14,12 @@
 import numpy as np
 import carla
 
+from opencda.core.common.misc import get_speed, positive, cal_distance_angle
 from opencda.core.plan.collision_check import CollisionChecker
 from opencda.core.plan.local_planner_behavior import LocalPlanner, RoadOption
 from opencda.core.plan.global_route_planner import GlobalRoutePlanner
 from opencda.core.plan.global_route_planner_dao import GlobalRoutePlannerDAO
-from opencda.core.common.misc import get_speed, positive, cal_distance_angle
+from opencda.core.plan.planer_debug_helper import PlanDebugHelper
 
 
 class BehaviorAgent(object):
@@ -51,6 +52,7 @@ def __init__(self, vehicle, carla_map, config_yaml):
  self.safety_time = config_yaml['safety_time']
  self.emergency_param = config_yaml['emergency_param']
  self.break_distance = 0
+ self.ttc = 1000
  # collision checker
  self._collision_check = CollisionChecker(time_ahead=config_yaml['collision_time_ahead'])
  self.ignore_traffic_light = config_yaml['ignore_traffic_light']
@@ -83,6 +85,9 @@ def __init__(self, vehicle, carla_map, config_yaml):
  self.white_list = []
  self.obstacle_vehicles = []
 
+ # debug helper
+ self.debug_helper = PlanDebugHelper(self.vehicle.id)
+
  def update_information(self, ego_pos, ego_speed, objects):
  """
  Update the perception and localization information to the behavior agent.
@@ -102,6 +107,9 @@ def update_information(self, ego_pos, ego_speed, objects):
  obstacle_vehicles = objects['vehicles']
  self.obstacle_vehicles = self.white_list_match(obstacle_vehicles)
 
+ # update the debug helper
+ self.debug_helper.update(ego_speed, self.ttc)
+
  if self.ignore_traffic_light:
  self.light_state = "Green"
  else:
@@ -402,6 +410,7 @@ def car_following_manager(self, vehicle, distance, target_speed=None):
 
  delta_v = max(1, (self._ego_speed - vehicle_speed) / 3.6)
  ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.)
+ self.ttc = ttc
  # Under safety time distance, slow down.
  if self.safety_time > ttc > 0.0:
  target_speed = min(positive(vehicle_speed - self.speed_decrease),
@@ -428,6 +437,8 @@ def run_step(self, target_speed=None, collision_detector_enabled=True, lane_chan
  # retrieve ego location
  ego_vehicle_loc = self._ego_pos.location
  ego_vehicle_wp = self._map.get_waypoint(ego_vehicle_loc)
+ # ttc reset to 1000 at the beginning
+ self.ttc = 1000
 
  # simulation ends condition
  if abs(self._ego_pos.location.x - self.end_waypoint.transform.location.x) <= 10 and \

opencda/scenario_testing/utils/profile_plotting.py → opencda/core/plan/drive_profile_plotting.py

@@ -10,128 +10,108 @@
 import numpy as np
 
 
-def draw_velocity_profile_single_plot(velocity_list, show=False):
+def draw_velocity_profile_single_plot(velocity_list):
  """
  Draw velocity profiles in a single plot
- :param show:
  :param velocity_list:
  :return:
  """
 
- label = []
-
  for i, v in enumerate(velocity_list):
  x_s = np.arange(len(v)) * 0.05
- label.append('Leading Vehicle, id: %d' % i if i == 0 else 'Following Vehicle, id: %d' % i)
  plt.plot(x_s, v)
 
- plt.ylim([10, 34])
+ plt.ylim([0, 34])
 
  plt.xlabel("Time (s)")
  plt.ylabel("Speed (m/s)")
  fig = plt.gcf()
  fig.set_size_inches(11, 5)
 
- if show:
- plt.legend(label)
- plt.show()
 
-
-def draw_acceleration_profile_single_plot(acceleration, show=False):
+def draw_acceleration_profile_single_plot(acceleration):
  """
  Draw velocity profiles in a single plot
- :param show:
  :param acceleration:
  :return:
  """
 
- label = []
-
  for i, v in enumerate(acceleration):
  x_s = np.arange(len(v)) * 0.05
- label.append('Leading Vehicle, id: %d' % i if i == 0 else 'Following Vehicle, id: %d' % i)
  plt.plot(x_s, v)
 
- plt.ylim([-8, 5])
+ plt.ylim([-8, 8])
 
  plt.xlabel("Time (s)")
  plt.ylabel("Acceleration (m^2/s)")
  fig = plt.gcf()
  fig.set_size_inches(11, 5)
 
- if show:
- plt.legend(label)
- plt.show()
+
+def draw_ttc_profile_single_plot(ttc_list):
+ """
+ Draw ttc.
+ :param ttc_list: ttc
+ :return:
+ """
+ # this is used to find the merging vehicle position since its inter gap length is always smaller
+
+ for i, v in enumerate(ttc_list):
+ x_s = np.arange(len(v)) * 0.05
+ plt.plot(x_s, v)
+
+ plt.xlabel("Time (s)")
+ plt.ylabel("TTC (s)")
+ plt.ylim([0, 30])
+ fig = plt.gcf()
+ fig.set_size_inches(11, 5)
 
 
-def draw_time_gap_profile_singel_plot(gap_list, show=False):
+def draw_time_gap_profile_singel_plot(gap_list):
  """
  Draw inter gap profiles in a single plot
  :param gap_list: time gap
  :return:
  """
- # this is used to find the merging vehicle position since its inter gap length is always smaller
- max_len = max(len(gap_list[0]), len(gap_list[-1]))
- label = []
 
  for i, v in enumerate(gap_list):
- if len(v) < max_len:
- x_s = np.arange(max_len - len(v), max_len) * 0.05
- else:
- x_s = np.arange(len(v)) * 0.05
+ x_s = np.arange(len(v)) * 0.05
  plt.plot(x_s, v)
- label.append('Leading Vehicle, id: %d' % i if i == 0 else 'Following Vehicle, id: %d' % i)
 
  plt.xlabel("Time (s)")
  plt.ylabel("Time Gap (s)")
  plt.ylim([0.0, 1.8])
  fig = plt.gcf()
  fig.set_size_inches(11, 5)
 
- if show:
- plt.legend(label)
- plt.show()
-
 
-def draw_dist_gap_profile_singel_plot(gap_list, show=False):
+def draw_dist_gap_profile_singel_plot(gap_list):
  """
  Draw distance gap profiles in a single plot
  :param gap_list: time gap
  :return:
  """
- # this is used to find the merging vehicle position since its inter gap length is always smaller
- max_len = max(len(gap_list[0]), len(gap_list[-1]))
- label = []
-
  for i, v in enumerate(gap_list):
- if len(v) < max_len:
- x_s = np.arange(max_len - len(v), max_len) * 0.05
- else:
- x_s = np.arange(len(v)) * 0.05
+ x_s = np.arange(len(v)) * 0.05
  plt.plot(x_s, v)
- label.append('Leading Vehicle, id: %d' % i if i == 0 else 'Following Vehicle, id: %d' % i)
 
  plt.xlabel("Time (s)")
  plt.ylabel("Distance Gap (m)")
  plt.ylim([5, 45])
  fig = plt.gcf()
  fig.set_size_inches(11, 5)
 
- if show:
- plt.legend(label)
- plt.show()
-
 
 def draw_sub_plot(velocity_list, acceleration_list, time_gap_list, distance_gap_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 velocity_list:
  :return:
  """
- fig = plt.figure(figsize=[2200,1000])
+ fig = plt.figure(figsize=[2200, 1000])
  plt.subplot(411)
  draw_velocity_profile_single_plot(velocity_list)
 

opencda/core/plan/planer_debug_helper.py

@@ -0,0 +1,80 @@
+# -*- coding: utf-8 -*-
+"""
+Analysis + Visualization functions for planning
+"""
+# Author: Runsheng Xu <[email protected]>
+# License: MIT
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+import opencda.core.plan.drive_profile_plotting as open_plt
+
+
+class PlanDebugHelper(object):
+ """This class aims to save statistics for planner behaviour
+ Attributes:
+ speed_list (list): The list containing speed info(m/s) of all time-steps
+ acc_list(list): The list containing acceleration info(m^2/s) of all time-steps
+ ttc_list(list): The list containing ttc info(s) for all time-steps
+ """
+
+ def __init__(self, actor_id):
+ self.actor_id = actor_id
+ self.speed_list = [[]]
+ self.acc_list = [[]]
+ self.ttc_list = [[]]
+
+ def update(self, ego_speed, ttc):
+ """
+ Update the speed info.
+ Args:
+ ego_speed(km/h): Ego speed.
+ ttc(s): time to collision.
+ Returns:
+
+ """
+ self.speed_list[0].append(ego_speed / 3.6)
+ 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)
+
+ def evaluate(self):
+ # draw speed, acc and ttc plotting
+ figure = plt.figure()
+ plt.subplot(311)
+ open_plt.draw_velocity_profile_single_plot(self.speed_list)
+
+ plt.subplot(312)
+ open_plt.draw_acceleration_profile_single_plot(self.acc_list)
+
+ plt.subplot(313)
+ open_plt.draw_ttc_profile_single_plot(self.ttc_list)
+
+ figure.suptitle('planning profile of actor id %d' % self.actor_id)
+
+ # calculate the statistics
+ spd_avg = np.mean(np.array(self.speed_list[0]))
+ spd_std = np.std(np.array(self.speed_list[0]))
+
+ acc_avg = np.mean(np.array(self.acc_list[0]))
+ acc_std = np.std(np.array(self.acc_list[0]))
+
+ ttc_array = np.array(self.ttc_list[0])
+ ttc_array = ttc_array[ttc_array < 1000]
+ ttc_avg = np.mean(ttc_array)
+ ttc_std = np.std(ttc_array)
+
+ perform_txt = 'Speed average: %f (m/s), ' \
+ 'Speed std: %f (m/s) \n' % (spd_avg, spd_std)
+
+ perform_txt += 'Acceleration average: %f (m/s), ' \
+ 'Acceleration std: %f (m/s) \n' % (acc_avg, acc_std)
+
+ perform_txt += 'TTC average: %f (m/s), ' \
+ 'TTC std: %f (m/s) \n' % (ttc_avg, ttc_std)
+
+ return figure, perform_txt

opencda/core/sensing/localization/localization_debug_helper.py

@@ -5,9 +5,6 @@
 # Author: Runsheng Xu <[email protected]>
 # License: MIT
 
-import os
-
-import cv2
 import numpy as np
 import matplotlib
 

opencda/scenario_testing/evaluations/evaluate_manager.py

@@ -47,6 +47,32 @@ def evaluate(self):
  self.localization_eval(log_file)
  print('Localization Evaluation Done.')
 
+ self.kinematics_eval(log_file)
+ print('Kinematics Evaluation Done.')
+
+ def kinematics_eval(self, log_file):
+ """
+ vehicle kinematics related evaluation.
+ Args:
+ log_file (File): The log file to write the data.
+
+ Returns:
+
+ """
+ lprint(log_file, "***********Kinematics Module***********")
+ for vid, vm in self.cav_world.get_vehicle_managers().items():
+ actor_id = vm.vehicle.id
+ lprint(log_file, 'Actor ID: %d' % actor_id)
+
+ loc_debug_helper = vm.agent.debug_helper
+ figure, perform_txt = loc_debug_helper.evaluate()
+
+ # save plotting
+ figure_save_path = os.path.join(self.eval_save_path, '%d_kinematics_plotting.png' % actor_id)
+ figure.savefig(figure_save_path, dpi=100)
+
+ lprint(log_file, perform_txt)
+
  def localization_eval(self, log_file):
  """
  Localization module evaluation.