Esempio n. 1
0
    def __init__(self, is_simulation, is_sim):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_simulation = is_simulation
        self.is_sim = is_sim
        self.hard_break_list = []
        self.total_cycle_num = 0
        self.init_point_curvature = []
        self.init_point_dcurvature = []
        self.init_point_accel = []
        self.init_point_decel = []

        self.last_init_point_t = None
        self.last_init_point = None

        self.centripetal_jerk_list = []

        self.breaking_2_3_cnt = 0
        self.breaking_3_5_cnt = 0
        self.breaking_5_cnt = 0
        self.throttle_2_3_cnt = 0
        self.throttle_3_5_cnt = 0
        self.throttle_5_cnt = 0
 def __init__(self):
     """init"""
     self.module_latency = []
     self.trajectory_type_dist = {}
     self.estop_reason_dist = {}
     self.error_code_analyzer = ErrorCodeAnalyzer()
     self.error_msg_analyzer = ErrorMsgAnalyzer()
class ControlAnalyzer:
    """control analyzer"""
    def __init__(self):
        """init"""
        self.module_latency = []
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()

    def put(self, control_cmd):
        """put data"""
        latency = control_cmd.latency_stats.total_time_ms
        self.module_latency.append(latency)
        self.error_code_analyzer.put(control_cmd.header.status.error_code)
        self.error_msg_analyzer.put(control_cmd.header.status.msg)

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Control Latency (ms) ---" + \
              PrintColors.ENDC
        analyzer = StatisticalAnalyzer()
        analyzer.print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- Control Error Code Distribution ---" + \
              PrintColors.ENDC
        self.error_code_analyzer.print_results()

        print PrintColors.HEADER + "--- Control Error Msg Distribution ---" + \
              PrintColors.ENDC
        self.error_msg_analyzer.print_results()
Esempio n. 4
0
 def __init__(self):
     """init"""
     self.module_latency = []
     self.error_code_analyzer = ErrorCodeAnalyzer()
     self.error_msg_analyzer = ErrorMsgAnalyzer()
     self.lon_station_error = []
     self.lon_speed_error = []
     self.lat_lateral_error = []
     self.lat_heading_error = []
class ControlAnalyzer:
    """control analyzer"""

    def __init__(self):
        """init"""
        self.module_latency = []
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.lon_station_error = []
        self.lon_speed_error = []
        self.lat_lateral_error = []
        self.lat_heading_error = []

    def put(self, control_cmd):
        """put data"""
        latency = control_cmd.latency_stats.total_time_ms
        self.module_latency.append(latency)
        self.error_code_analyzer.put(control_cmd.header.status.error_code)
        self.error_msg_analyzer.put(control_cmd.header.status.msg)
        lon_debug = control_cmd.debug.simple_lon_debug
        self.lon_station_error.append(lon_debug.station_error)
        self.lon_speed_error.append(lon_debug.speed_error)
        lat_debug = control_cmd.debug.simple_lat_debug
        self.lat_lateral_error.append(lat_debug.lateral_error)
        self.lat_heading_error.append(lat_debug.heading_error)

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Control Latency (ms) ---" + \
            PrintColors.ENDC
        analyzer = StatisticalAnalyzer()
        analyzer.print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- station error ---" + \
            PrintColors.ENDC
        analyzer.print_statistical_results(self.lon_station_error)

        print PrintColors.HEADER + "--- speed error ---" + \
            PrintColors.ENDC
        analyzer.print_statistical_results(self.lon_speed_error)

        print PrintColors.HEADER + "--- lateral error ---" + \
            PrintColors.ENDC
        analyzer.print_statistical_results(self.lat_lateral_error)

        print PrintColors.HEADER + "--- heading error ---" + \
            PrintColors.ENDC
        analyzer.print_statistical_results(self.lat_heading_error)

        print PrintColors.HEADER + "--- Control Error Code Distribution ---" + \
            PrintColors.ENDC
        self.error_code_analyzer.print_results()

        print PrintColors.HEADER + "--- Control Error Msg Distribution ---" + \
            PrintColors.ENDC
        self.error_msg_analyzer.print_results()
Esempio n. 6
0
 def __init__(self):
     """init"""
     self.module_latency = []
     self.trajectory_type_dist = {}
     self.estop_reason_dist = {}
     self.error_code_analyzer = ErrorCodeAnalyzer()
     self.error_msg_analyzer = ErrorMsgAnalyzer()
     self.last_adc_trajectory = None
     self.frechet_distance_list = []
 def __init__(self, is_simulation):
     """init"""
     self.module_latency = []
     self.trajectory_type_dist = {}
     self.estop_reason_dist = {}
     self.error_code_analyzer = ErrorCodeAnalyzer()
     self.error_msg_analyzer = ErrorMsgAnalyzer()
     self.last_adc_trajectory = None
     self.frechet_distance_list = []
     self.is_simulation = is_simulation
     self.hard_break_list = []
     self.total_cycle_num = 0
class PlannigAnalyzer:
    """planning analyzer"""

    def __init__(self):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()

    def put(self, adc_trajectory):
        """put"""
        latency = adc_trajectory.latency_stats.total_time_ms
        self.module_latency.append(latency)

        self.error_code_analyzer.put(adc_trajectory.header.status.error_code)
        self.error_msg_analyzer.put(adc_trajectory.header.status.msg)

        traj_type = planning_pb2.ADCTrajectory.TrajectoryType.Name(
            adc_trajectory.trajectory_type)
        self.trajectory_type_dist[traj_type] = \
            self.trajectory_type_dist.get(traj_type, 0) + 1

        if adc_trajectory.estop.is_estop:
            self.estop_reason_dist[adc_trajectory.estop.reason] = \
                self.estop_reason_dist.get(adc_trajectory.estop.reason, 0) + 1

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Planning Latency (ms) ---" + \
              PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- Planning Trajectroy Type Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.trajectory_type_dist)

        print PrintColors.HEADER + "--- Planning Estop Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.estop_reason_dist)

        print PrintColors.HEADER + "--- Planning Error Code Distribution---" + \
              PrintColors.ENDC
        self.error_code_analyzer.print_results()
        print PrintColors.HEADER + "--- Planning Error Msg Distribution ---" + \
              PrintColors.ENDC
        self.error_msg_analyzer.print_results()
 def __init__(self, is_simulation):
     """init"""
     self.module_latency = []
     self.trajectory_type_dist = {}
     self.estop_reason_dist = {}
     self.error_code_analyzer = ErrorCodeAnalyzer()
     self.error_msg_analyzer = ErrorMsgAnalyzer()
     self.last_adc_trajectory = None
     self.frechet_distance_list = []
     self.is_simulation = is_simulation
     self.hard_break_list = []
Esempio n. 10
0
    def __init__(self, arguments):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_sim = arguments.simulation
        self.hard_break_list = []
        self.total_cycle_num = 0

        self.curvature_analyzer = Curvature()
        self.frame_count_analyzer = FrameCount()
        self.lat_acceleration_analyzer = LatAcceleration()
        self.lon_acceleration_analyzer = LonAcceleration()
        self.latency_analyzer = Latency()
        self.reference_line = ReferenceLine()

        self.bag_start_time_t = None
        self.print_acc = arguments.showacc
Esempio n. 11
0
class PlannigAnalyzer:
    """planning analyzer"""
    def __init__(self, arguments):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_sim = arguments.simulation
        self.hard_break_list = []
        self.total_cycle_num = 0

        self.curvature_analyzer = Curvature()
        self.frame_count_analyzer = FrameCount()
        self.lat_acceleration_analyzer = LatAcceleration()
        self.lon_acceleration_analyzer = LonAcceleration()
        self.latency_analyzer = Latency()
        self.reference_line = ReferenceLine()

        self.bag_start_time_t = None
        self.print_acc = arguments.showacc

    def put(self, adc_trajectory):
        self.total_cycle_num += 1
        if not self.is_sim:
            latency = adc_trajectory.latency_stats.total_time_ms
            self.module_latency.append(latency)

            self.error_code_analyzer.put(
                adc_trajectory.header.status.error_code)
            self.error_msg_analyzer.put(adc_trajectory.header.status.msg)

            traj_type = planning_pb2.ADCTrajectory.TrajectoryType.Name(
                adc_trajectory.trajectory_type)
            self.trajectory_type_dist[traj_type] = \
                self.trajectory_type_dist.get(traj_type, 0) + 1

            if adc_trajectory.estop.is_estop:
                self.estop_reason_dist[adc_trajectory.estop.reason] = \
                    self.estop_reason_dist.get(
                        adc_trajectory.estop.reason, 0) + 1

        else:
            self.curvature_analyzer.put(adc_trajectory)
            self.frame_count_analyzer.put(adc_trajectory)
            self.lat_acceleration_analyzer.put(adc_trajectory)
            self.lon_acceleration_analyzer.put(adc_trajectory)
            self.latency_analyzer.put(adc_trajectory)
            self.reference_line.put(adc_trajectory)

    def find_common_path(self, current_adc_trajectory, last_adc_trajectory):
        current_path_points = current_adc_trajectory.trajectory_point
        last_path_points = last_adc_trajectory.trajectory_point

        current_path = []
        for point in current_path_points:
            current_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break
        last_path = []
        for point in last_path_points:
            last_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break

        if len(current_path) == 0 or len(last_path) == 0:
            return [], []

        current_ls = LineString(current_path)
        last_ls = LineString(last_path)
        current_start_point = Point(current_path[0])

        dist = last_ls.project(current_start_point)
        cut_lines = self.cut(last_ls, dist)
        if len(cut_lines) == 1:
            return [], []
        last_ls = cut_lines[1]
        dist = current_ls.project(Point(last_path[-1]))
        if dist <= current_ls.length:
            current_ls = self.cut(current_ls, dist)[0]
        else:
            dist = last_ls.project(Point(current_path[-1]))
            last_ls = self.cut(last_ls, dist)[0]
        return current_ls.coords, last_ls.coords

    def cut(self, line, distance):
        if distance <= 0.0 or distance >= line.length:
            return [LineString(line)]
        coords = list(line.coords)
        for i, p in enumerate(coords):
            pd = line.project(Point(p))
            if pd == distance:
                return [LineString(coords[:i + 1]), LineString(coords[i:])]
            if pd > distance:
                cp = line.interpolate(distance)
                return [
                    LineString(coords[:i] + [(cp.x, cp.y)]),
                    LineString([(cp.x, cp.y)] + coords[i:])
                ]

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print("\n\n")
        print(PrintColors.HEADER + "--- Planning Latency (ms) ---" + \
            PrintColors.ENDC)
        StatisticalAnalyzer().print_statistical_results(self.module_latency)

        print(PrintColors.HEADER + "--- Planning Trajectroy Type Distribution" \
                                   " ---" + PrintColors.ENDC)
        DistributionAnalyzer().print_distribution_results(
            self.trajectory_type_dist)

        print(PrintColors.HEADER + "--- Planning Estop Distribution" \
                                   " ---" + PrintColors.ENDC)
        DistributionAnalyzer().print_distribution_results(
            self.estop_reason_dist)

        print(PrintColors.HEADER + "--- Planning Error Code Distribution---" + \
            PrintColors.ENDC)
        self.error_code_analyzer.print_results()
        print(PrintColors.HEADER + "--- Planning Error Msg Distribution ---" + \
            PrintColors.ENDC)
        self.error_msg_analyzer.print_results()

        print(PrintColors.HEADER + "--- Planning Trajectory Frechet Distance (m) ---" + \
            PrintColors.ENDC)
        StatisticalAnalyzer().print_statistical_results(
            self.frechet_distance_list)

    def print_sim_results(self):
        """
        dreamland metrics for planning v2
        """
        v2_results = {}

        # acceleration
        v2_results["accel"] = self.lon_acceleration_analyzer.get_acceleration()

        # deceleration
        v2_results["decel"] = self.lon_acceleration_analyzer.get_deceleration()

        # jerk
        v2_results["acc_jerk"] = self.lon_acceleration_analyzer.get_acc_jerk()
        v2_results["dec_jerk"] = self.lon_acceleration_analyzer.get_dec_jerk()

        # centripetal_jerk
        v2_results["lat_jerk"] = self.lat_acceleration_analyzer.get_jerk()

        # centripetal_accel
        v2_results[
            "lat_accel"] = self.lat_acceleration_analyzer.get_acceleration()

        # frame_count
        v2_results["frame_count"] = self.frame_count_analyzer.get()

        # latency
        v2_results["planning_latency"] = self.latency_analyzer.get()

        # reference line
        v2_results["reference_line"] = self.reference_line.get()

        # output final reuslts
        print(json.dumps(v2_results))

    def plot_path(self, plt, adc_trajectory):
        path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)
        x = []
        y = []
        for point in path_coords:
            x.append(point[0])
            y.append(point[1])
        plt.plot(x, y, 'r-', alpha=0.5)

    def plot_refpath(self, plt, adc_trajectory):
        for path in adc_trajectory.debug.planning_data.path:
            if path.name != 'planning_reference_line':
                continue
            path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)

            ref_path_coord = []
            for point in path.path_point:
                ref_path_coord.append([point.x, point.y])
            ref_path = LineString(ref_path_coord)

            start_point = Point(path_coords[0])
            dist = ref_path.project(start_point)
            ref_path = self.cut(ref_path, dist)[1]

            end_point = Point(path_coords[-1])
            dist = ref_path.project(end_point)
            ref_path = self.cut(ref_path, dist)[0]

            x = []
            y = []
            for point in ref_path.coords:
                x.append(point[0])
                y.append(point[1])

            plt.plot(x, y, 'b--', alpha=0.5)

    def trim_path_by_distance(self, adc_trajectory, s):
        path_coords = []
        path_points = adc_trajectory.trajectory_point
        for point in path_points:
            if point.path_point.s <= s:
                path_coords.append([point.path_point.x, point.path_point.y])
        return path_coords
Esempio n. 12
0
 def __init__(self):
     """init"""
     self.module_latency = []
     self.error_code_analyzer = ErrorCodeAnalyzer()
     self.error_msg_analyzer = ErrorMsgAnalyzer()
Esempio n. 13
0
    def __init__(self, is_simulation, is_sim):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_simulation = is_simulation
        self.is_sim = is_sim
        self.hard_break_list = []
        self.total_cycle_num = 0

        self.init_point_curvature = []
        self.init_point_dcurvature = []
        self.init_point_accel = []
        self.init_point_decel = []

        self.last_init_point_t = None
        self.last_init_point_a = None
        self.last_init_point = None

        self.centripetal_jerk_list = []
        self.centripetal_accel_list = []
        self.jerk_list = []

        # [2, 4) unit m/s^2
        self.ACCEL_M_LB = 2
        self.ACCEL_M_UB = 4
        self.accel_medium_cnt = 0

        # [4, ) unit m/s^2
        self.ACCEL_H_LB = 4
        self.accel_high_cnt = 0

        # [-4, -2)
        self.DECEL_M_LB = -4
        self.DECEL_M_UB = -2
        self.decel_medium_cnt = 0

        # [-4, )
        self.DECEL_H_UB = -4
        self.decel_high_cnt = 0

        # [1,2) (-2, -1]
        self.JERK_M_LB_P = 1
        self.JERK_M_UB_P = 2
        self.JERK_M_LB_N = -2
        self.JERK_M_UB_N = -1
        self.jerk_medium_cnt = 0

        # [2, inf) (-inf, -2]
        self.JERK_H_LB_P = 2
        self.JERK_H_UB_N = -2
        self.jerk_high_cnt = 0

        # [1, 2) [-2, -1)
        self.LAT_ACCEL_M_LB_P = 1
        self.LAT_ACCEL_M_UB_P = 2
        self.LAT_ACCEL_M_LB_N = -2
        self.LAT_ACCEL_M_UB_N = -1
        self.lat_accel_medium_cnt = 0

        # [2, inf)  [-inf,-2)
        self.LAT_ACCEL_H_LB_P = 2
        self.LAT_ACCEL_H_UB_N = -2
        self.lat_accel_high_cnt = 0

        # [0.5,1) [-1, -0.5)
        self.LAT_JERK_M_LB_P = 0.5
        self.LAT_JERK_M_UB_P = 1
        self.LAT_JERK_M_LB_N = -1
        self.LAT_JERK_M_UB_N = -0.5
        self.lat_jerk_medium_cnt = 0

        # [1, inf)  [-inf,-1)
        self.LAT_JERK_H_LB_P = 1
        self.LAT_JERK_H_UB_N = -1
        self.lat_jerk_high_cnt = 0
Esempio n. 14
0
class PlannigAnalyzer:
    """planning analyzer"""

    def __init__(self, is_simulation):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_simulation = is_simulation
        self.hard_break_list = []

    def put(self, adc_trajectory):
        """put"""
        if not self.is_simulation:
            latency = adc_trajectory.latency_stats.total_time_ms
            self.module_latency.append(latency)

            self.error_code_analyzer.put(
                adc_trajectory.header.status.error_code)
            self.error_msg_analyzer.put(adc_trajectory.header.status.msg)

            traj_type = planning_pb2.ADCTrajectory.TrajectoryType.Name(
                adc_trajectory.trajectory_type)
            self.trajectory_type_dist[traj_type] = \
                self.trajectory_type_dist.get(traj_type, 0) + 1

            if adc_trajectory.estop.is_estop:
                self.estop_reason_dist[adc_trajectory.estop.reason] = \
                    self.estop_reason_dist.get(
                        adc_trajectory.estop.reason, 0) + 1

        if self.is_simulation:
            for point in adc_trajectory.trajectory_point:
                if point.a <= -2.0:
                    self.hard_break_list.append(point.a)

        if self.last_adc_trajectory is not None:
            current_path, last_path = self.find_common_path(adc_trajectory,
                                                            self.last_adc_trajectory)
            if len(current_path) == 0 or len(last_path) == 0:
                dist = 0
            else:
                dist = frechet_distance(current_path, last_path)
                self.frechet_distance_list.append(dist)

        self.last_adc_trajectory = adc_trajectory

    def find_common_path(self, current_adc_trajectory, last_adc_trajectory):
        current_path_points = current_adc_trajectory.trajectory_point
        last_path_points = last_adc_trajectory.trajectory_point
        current_path = []
        for point in current_path_points:
            current_path.append([point.path_point.x, point.path_point.y])
        last_path = []
        for point in last_path_points:
            last_path.append([point.path_point.x, point.path_point.y])
        if len(current_path) == 0 or len(last_path) == 0:
            return [], []

        current_ls = LineString(current_path)
        last_ls = LineString(last_path)
        current_start_point = Point(current_path[0])

        dist = last_ls.project(current_start_point)
        cut_lines = self.cut(last_ls, dist)
        if len(cut_lines) == 1:
            return [], []
        last_ls = cut_lines[1]
        dist = current_ls.project(Point(last_path[-1]))
        if dist <= current_ls.length:
            current_ls = self.cut(current_ls, dist)[0]
        else:
            dist = last_ls.project(Point(current_path[-1]))
            last_ls = self.cut(last_ls, dist)[0]
        return current_ls.coords, last_ls.coords

    def cut(self, line, distance):
        if distance <= 0.0 or distance >= line.length:
            return [LineString(line)]
        coords = list(line.coords)
        for i, p in enumerate(coords):
            pd = line.project(Point(p))
            if pd == distance:
                return [
                    LineString(coords[:i+1]),
                    LineString(coords[i:])]
            if pd > distance:
                cp = line.interpolate(distance)
                return [
                    LineString(coords[:i] + [(cp.x, cp.y)]),
                    LineString([(cp.x, cp.y)] + coords[i:])]

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Planning Latency (ms) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- Planning Trajectroy Type Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.trajectory_type_dist)

        print PrintColors.HEADER + "--- Planning Estop Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.estop_reason_dist)

        print PrintColors.HEADER + "--- Planning Error Code Distribution---" + \
            PrintColors.ENDC
        self.error_code_analyzer.print_results()
        print PrintColors.HEADER + "--- Planning Error Msg Distribution ---" + \
            PrintColors.ENDC
        self.error_msg_analyzer.print_results()

        print PrintColors.HEADER + "--- Planning Trajectory Frechet Distance (m) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.frechet_distance_list)

    def print_simulation_results(self):
        results = {}
        results['frechet_dist'] = sum(self.frechet_distance_list) /\
            len(self.frechet_distance_list)
        results['hard_brake_cycle_num'] = len(self.hard_break_list)
        print str(results)
class PlannigAnalyzer:
    """planning analyzer"""

    def __init__(self, is_simulation):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_simulation = is_simulation
        self.hard_break_list = []
        self.total_cycle_num = 0

    def put(self, adc_trajectory):
        self.total_cycle_num += 1
        """put"""
        if not self.is_simulation:
            latency = adc_trajectory.latency_stats.total_time_ms
            self.module_latency.append(latency)

            self.error_code_analyzer.put(
                adc_trajectory.header.status.error_code)
            self.error_msg_analyzer.put(adc_trajectory.header.status.msg)

            traj_type = planning_pb2.ADCTrajectory.TrajectoryType.Name(
                adc_trajectory.trajectory_type)
            self.trajectory_type_dist[traj_type] = \
                self.trajectory_type_dist.get(traj_type, 0) + 1

            if adc_trajectory.estop.is_estop:
                self.estop_reason_dist[adc_trajectory.estop.reason] = \
                    self.estop_reason_dist.get(
                        adc_trajectory.estop.reason, 0) + 1

        if self.is_simulation:
            for point in adc_trajectory.trajectory_point:
                if point.a <= -2.0:
                    self.hard_break_list.append(point.a)

        if self.last_adc_trajectory is not None:
            current_path, last_path = self.find_common_path(adc_trajectory,
                                                            self.last_adc_trajectory)
            if len(current_path) == 0 or len(last_path) == 0:
                dist = 0
            else:
                dist = frechet_distance(current_path, last_path)
                self.frechet_distance_list.append(dist)

        self.last_adc_trajectory = adc_trajectory

    def find_common_path(self, current_adc_trajectory, last_adc_trajectory):
        current_path_points = current_adc_trajectory.trajectory_point
        last_path_points = last_adc_trajectory.trajectory_point
        current_path = []
        for point in current_path_points:
            current_path.append([point.path_point.x, point.path_point.y])
        last_path = []
        for point in last_path_points:
            last_path.append([point.path_point.x, point.path_point.y])
        if len(current_path) == 0 or len(last_path) == 0:
            return [], []

        current_ls = LineString(current_path)
        last_ls = LineString(last_path)
        current_start_point = Point(current_path[0])

        dist = last_ls.project(current_start_point)
        cut_lines = self.cut(last_ls, dist)
        if len(cut_lines) == 1:
            return [], []
        last_ls = cut_lines[1]
        dist = current_ls.project(Point(last_path[-1]))
        if dist <= current_ls.length:
            current_ls = self.cut(current_ls, dist)[0]
        else:
            dist = last_ls.project(Point(current_path[-1]))
            last_ls = self.cut(last_ls, dist)[0]
        return current_ls.coords, last_ls.coords

    def cut(self, line, distance):
        if distance <= 0.0 or distance >= line.length:
            return [LineString(line)]
        coords = list(line.coords)
        for i, p in enumerate(coords):
            pd = line.project(Point(p))
            if pd == distance:
                return [
                    LineString(coords[:i+1]),
                    LineString(coords[i:])]
            if pd > distance:
                cp = line.interpolate(distance)
                return [
                    LineString(coords[:i] + [(cp.x, cp.y)]),
                    LineString([(cp.x, cp.y)] + coords[i:])]

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Planning Latency (ms) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- Planning Trajectroy Type Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.trajectory_type_dist)

        print PrintColors.HEADER + "--- Planning Estop Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.estop_reason_dist)

        print PrintColors.HEADER + "--- Planning Error Code Distribution---" + \
            PrintColors.ENDC
        self.error_code_analyzer.print_results()
        print PrintColors.HEADER + "--- Planning Error Msg Distribution ---" + \
            PrintColors.ENDC
        self.error_msg_analyzer.print_results()

        print PrintColors.HEADER + "--- Planning Trajectory Frechet Distance (m) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.frechet_distance_list)

    def print_simulation_results(self):
        results = {}
        results['frechet_dist'] = sum(self.frechet_distance_list) /\
            len(self.frechet_distance_list)
        results['hard_brake_cycle_num'] = len(self.hard_break_list)
        results['overall_score'] = 1- results['hard_brake_cycle_num'] /\
            float(self.total_cycle_num)
        if results['frechet_dist'] > 10:
            results['overall_score'] += 0.0
        else:
            results['overall_score'] += (1- results['frechet_dist'] / 10.0)
        results['overall_score'] /= 2.0
        

        print str(results)
Esempio n. 16
0
class PlannigAnalyzer:
    """planning analyzer"""
    def __init__(self, is_simulation, is_sim):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_simulation = is_simulation
        self.is_sim = is_sim
        self.hard_break_list = []
        self.total_cycle_num = 0
        self.init_point_curvature = []
        self.init_point_dcurvature = []
        self.init_point_accel = []
        self.init_point_decel = []

        self.last_init_point_t = None
        self.last_init_point = None

        self.centripetal_jerk_list = []

        self.breaking_2_3_cnt = 0
        self.breaking_3_5_cnt = 0
        self.breaking_5_cnt = 0
        self.throttle_2_3_cnt = 0
        self.throttle_3_5_cnt = 0
        self.throttle_5_cnt = 0

    def put(self, adc_trajectory):
        self.total_cycle_num += 1
        if not self.is_simulation and not self.is_sim:
            latency = adc_trajectory.latency_stats.total_time_ms
            self.module_latency.append(latency)

            self.error_code_analyzer.put(
                adc_trajectory.header.status.error_code)
            self.error_msg_analyzer.put(adc_trajectory.header.status.msg)

            traj_type = planning_pb2.ADCTrajectory.TrajectoryType.Name(
                adc_trajectory.trajectory_type)
            self.trajectory_type_dist[traj_type] = \
                self.trajectory_type_dist.get(traj_type, 0) + 1

            if adc_trajectory.estop.is_estop:
                self.estop_reason_dist[adc_trajectory.estop.reason] = \
                    self.estop_reason_dist.get(
                        adc_trajectory.estop.reason, 0) + 1

        if self.is_simulation or self.is_sim:
            if adc_trajectory.debug.planning_data.HasField('init_point'):
                init_point = adc_trajectory.debug.planning_data.init_point

                self.init_point_curvature.append(
                    abs(init_point.path_point.kappa))
                self.init_point_dcurvature.append(
                    abs(init_point.path_point.dkappa))

                t = adc_trajectory.header.timestamp_sec + init_point.relative_time

                if self.last_init_point is not None:
                    duration = t - self.last_init_point_t
                    if duration <= 0:
                        accel = 0
                    else:
                        accel = (init_point.v -
                                 self.last_init_point.v) / duration
                    if accel > 0:
                        self.init_point_accel.append(accel)
                    if accel < 0:
                        self.init_point_decel.append(abs(accel))

                    if -3 < accel <= -2:
                        self.breaking_2_3_cnt += 1
                    if -5 < accel <= -3:
                        self.breaking_3_5_cnt += 1
                    if accel <= -5:
                        self.breaking_5_cnt += 1

                    if 2 <= accel < 3:
                        self.throttle_2_3_cnt += 1
                    if 3 <= accel < 5:
                        self.throttle_3_5_cnt += 1
                    if 5 <= accel:
                        self.throttle_5_cnt += 1

                    centripetal_jerk = 2 * init_point.v * init_point.a \
                        * init_point.path_point.kappa + init_point.v \
                            * init_point.v * init_point.path_point.dkappa
                    self.centripetal_jerk_list.append(centripetal_jerk)

                self.last_init_point_t = t
                self.last_init_point = init_point

        # TODO(yifei) temporarily disable frechet distance
        #if self.last_adc_trajectory is not None and self.is_simulation:
        #    current_path, last_path = self.find_common_path(adc_trajectory,
        #                                                    self.last_adc_trajectory)
        #    if len(current_path) == 0 or len(last_path) == 0:
        #        dist = 0
        #    else:
        #        dist = frechet_distance(current_path, last_path)
        #        if dist is not None:
        #            self.frechet_distance_list.append(dist)

        self.last_adc_trajectory = adc_trajectory

    def find_common_path(self, current_adc_trajectory, last_adc_trajectory):
        current_path_points = current_adc_trajectory.trajectory_point
        last_path_points = last_adc_trajectory.trajectory_point

        current_path = []
        for point in current_path_points:
            current_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break
        last_path = []
        for point in last_path_points:
            last_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break

        if len(current_path) == 0 or len(last_path) == 0:
            return [], []

        current_ls = LineString(current_path)
        last_ls = LineString(last_path)
        current_start_point = Point(current_path[0])

        dist = last_ls.project(current_start_point)
        cut_lines = self.cut(last_ls, dist)
        if len(cut_lines) == 1:
            return [], []
        last_ls = cut_lines[1]
        dist = current_ls.project(Point(last_path[-1]))
        if dist <= current_ls.length:
            current_ls = self.cut(current_ls, dist)[0]
        else:
            dist = last_ls.project(Point(current_path[-1]))
            last_ls = self.cut(last_ls, dist)[0]
        return current_ls.coords, last_ls.coords

    def cut(self, line, distance):
        if distance <= 0.0 or distance >= line.length:
            return [LineString(line)]
        coords = list(line.coords)
        for i, p in enumerate(coords):
            pd = line.project(Point(p))
            if pd == distance:
                return [LineString(coords[:i + 1]), LineString(coords[i:])]
            if pd > distance:
                cp = line.interpolate(distance)
                return [
                    LineString(coords[:i] + [(cp.x, cp.y)]),
                    LineString([(cp.x, cp.y)] + coords[i:])
                ]

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Planning Latency (ms) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- Planning Trajectroy Type Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.trajectory_type_dist)

        print PrintColors.HEADER + "--- Planning Estop Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.estop_reason_dist)

        print PrintColors.HEADER + "--- Planning Error Code Distribution---" + \
            PrintColors.ENDC
        self.error_code_analyzer.print_results()
        print PrintColors.HEADER + "--- Planning Error Msg Distribution ---" + \
            PrintColors.ENDC
        self.error_msg_analyzer.print_results()

        print PrintColors.HEADER + "--- Planning Trajectory Frechet Distance (m) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(
            self.frechet_distance_list)

    def print_simulation_results(self):
        results = {}

        results['decel_2_3'] = self.breaking_2_3_cnt
        results['decel_3_5'] = self.breaking_3_5_cnt
        results['decel_5_'] = self.breaking_5_cnt

        results['accel_2_3'] = self.throttle_2_3_cnt
        results['accel_3_5'] = self.throttle_3_5_cnt
        results['accel_5_'] = self.throttle_5_cnt

        if len(self.init_point_curvature) > 0:
            results['curvature_max'] = max(self.init_point_curvature, key=abs)
            curvature_avg = np.average(np.absolute(self.init_point_curvature))
            results['curvature_avg'] = curvature_avg
        else:
            results['curvature_max'] = None
            results['curvature_avg'] = None

        if len(self.init_point_dcurvature) > 0:
            results['dcurvature_max'] = max(self.init_point_dcurvature,
                                            key=abs)
            dcurvature_avg = np.average(np.absolute(
                self.init_point_dcurvature))
            results['dcurvature_avg'] = dcurvature_avg
        else:
            results['dcurvature_max'] = None
            results['dcurvature_avg'] = None

        if len(self.init_point_accel) > 0:
            results["accel_max"] = max(self.init_point_accel)
            results["accel_avg"] = np.average(self.init_point_accel)
        else:
            results["accel_max"] = 0.0
            results["accel_avg"] = 0.0

        if len(self.init_point_decel) > 0:
            results["decel_max"] = max(self.init_point_decel)
            results["decel_avg"] = np.average(self.init_point_decel)
        else:
            results["decel_max"] = 0.0
            results["decel_avg"] = 0.0

        if len(self.centripetal_jerk_list) > 0:
            results["centripetal_jerk_max"] = max(self.centripetal_jerk_list,
                                                  key=abs)
        else:
            results["centripetal_jerk_max"] = 0
        print json.dumps(results)

    def print_sim_results(self):
        """
        dreamland metrics for planning v2
        """
        v2_results = {}

        # acceleration
        v2_results["accel"] = {}
        if len(self.init_point_accel) > 0:
            v2_results["accel"]["max"] = max(self.init_point_accel)
            v2_results["accel"]["avg"] = np.average(self.init_point_accel)
        else:
            v2_results["accel"]["max"] = 0.0
            v2_results["accel"]["avg"] = 0.0
        v2_results["accel"]['2 to 3 cnt'] = self.throttle_2_3_cnt
        v2_results["accel"]['3 to 5 cnt'] = self.throttle_3_5_cnt
        v2_results["accel"]['5 and up cnt'] = self.throttle_5_cnt

        # deceleration
        v2_results["decel"] = {}
        if len(self.init_point_decel) > 0:
            v2_results["decel"]["max"] = max(self.init_point_decel)
            v2_results["decel"]["avg"] = np.average(self.init_point_decel)
        else:
            v2_results["decel"]["max"] = 0.0
            v2_results["decel"]["avg"] = 0.0
        v2_results["decel"]['2 to 3 cnt'] = self.breaking_2_3_cnt
        v2_results["decel"]['3 to 5 cnt'] = self.breaking_3_5_cnt
        v2_results["decel"]['5 and up cnt'] = self.breaking_5_cnt

        # centripetal_jerk
        v2_results["centripetal_jerk"] = {}
        if len(self.centripetal_jerk_list) > 0:
            v2_results["centripetal_jerk"]["max"] = max(
                self.centripetal_jerk_list, key=abs)
            jerk_avg = np.average(np.absolute(self.centripetal_jerk_list))
            v2_results["centripetal_jerk"]["avg"] = jerk_avg
        else:
            v2_results["centripetal_jerk"]["max"] = 0
            v2_results["centripetal_jerk"]["avg"] = 0

        print json.dumps(v2_results)

    def plot_path(self, plt, adc_trajectory):
        path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)
        x = []
        y = []
        for point in path_coords:
            x.append(point[0])
            y.append(point[1])
        plt.plot(x, y, 'r-', alpha=0.5)

    def plot_refpath(self, plt, adc_trajectory):
        for path in adc_trajectory.debug.planning_data.path:
            if path.name != 'planning_reference_line':
                continue
            path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)

            ref_path_coord = []
            for point in path.path_point:
                ref_path_coord.append([point.x, point.y])
            ref_path = LineString(ref_path_coord)

            start_point = Point(path_coords[0])
            dist = ref_path.project(start_point)
            ref_path = self.cut(ref_path, dist)[1]

            end_point = Point(path_coords[-1])
            dist = ref_path.project(end_point)
            ref_path = self.cut(ref_path, dist)[0]

            x = []
            y = []
            for point in ref_path.coords:
                x.append(point[0])
                y.append(point[1])

            plt.plot(x, y, 'b--', alpha=0.5)

    def trim_path_by_distance(self, adc_trajectory, s):
        path_coords = []
        path_points = adc_trajectory.trajectory_point
        for point in path_points:
            if point.path_point.s <= s:
                path_coords.append([point.path_point.x, point.path_point.y])
        return path_coords
Esempio n. 17
0
    def __init__(self, arguments):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_sim = arguments.simulation
        self.hard_break_list = []
        self.total_cycle_num = 0

        self.init_point_curvature = []
        self.init_point_dcurvature = []
        self.init_point_accel = []
        self.init_point_decel = []

        self.last_init_point_t = None
        self.last_init_point_a = None
        self.last_init_point = None

        self.centripetal_jerk_list = []
        self.centripetal_accel_list = []
        self.jerk_list = []

        self.latency_list = []

        # [2, 4) unit m/s^2
        self.ACCEL_M_LB = 2
        self.ACCEL_M_UB = 4
        self.accel_medium_cnt = 0

        # [4, ) unit m/s^2
        self.ACCEL_H_LB = 4
        self.accel_high_cnt = 0

        # [-4, -2)
        self.DECEL_M_LB = -4
        self.DECEL_M_UB = -2
        self.decel_medium_cnt = 0

        # [-4, )
        self.DECEL_H_UB = -4
        self.decel_high_cnt = 0

        # [1,2) (-2, -1]
        self.JERK_M_LB_P = 1
        self.JERK_M_UB_P = 2
        self.JERK_M_LB_N = -2
        self.JERK_M_UB_N = -1
        self.jerk_medium_cnt = 0

        # [2, inf) (-inf, -2]
        self.JERK_H_LB_P = 2
        self.JERK_H_UB_N = -2
        self.jerk_high_cnt = 0

        # [1, 2) [-2, -1)
        self.LAT_ACCEL_M_LB_P = 1
        self.LAT_ACCEL_M_UB_P = 2
        self.LAT_ACCEL_M_LB_N = -2
        self.LAT_ACCEL_M_UB_N = -1
        self.lat_accel_medium_cnt = 0

        # [2, inf)  [-inf,-2)
        self.LAT_ACCEL_H_LB_P = 2
        self.LAT_ACCEL_H_UB_N = -2
        self.lat_accel_high_cnt = 0

        # [0.5,1) [-1, -0.5)
        self.LAT_JERK_M_LB_P = 0.5
        self.LAT_JERK_M_UB_P = 1
        self.LAT_JERK_M_LB_N = -1
        self.LAT_JERK_M_UB_N = -0.5
        self.lat_jerk_medium_cnt = 0

        # [1, inf)  [-inf,-1)
        self.LAT_JERK_H_LB_P = 1
        self.LAT_JERK_H_UB_N = -1
        self.lat_jerk_high_cnt = 0

        self.bag_start_time_t = None
        self.print_acc = arguments.showacc

        self.rl_is_offroad_cnt = 0
        self.rl_minimum_boundary = sys.float_info.max
        self.rl_avg_kappa_list = []
        self.rl_avg_dkappa_list = []
Esempio n. 18
0
class PlannigAnalyzer:
    """planning analyzer"""
    def __init__(self, is_simulation, is_sim):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_simulation = is_simulation
        self.is_sim = is_sim
        self.hard_break_list = []
        self.total_cycle_num = 0

        self.init_point_curvature = []
        self.init_point_dcurvature = []
        self.init_point_accel = []
        self.init_point_decel = []

        self.last_init_point_t = None
        self.last_init_point_a = None
        self.last_init_point = None

        self.centripetal_jerk_list = []
        self.centripetal_accel_list = []
        self.jerk_list = []

        # [2, 4) unit m/s^2
        self.ACCEL_M_LB = 2
        self.ACCEL_M_UB = 4
        self.accel_medium_cnt = 0

        # [4, ) unit m/s^2
        self.ACCEL_H_LB = 4
        self.accel_high_cnt = 0

        # [-4, -2)
        self.DECEL_M_LB = -4
        self.DECEL_M_UB = -2
        self.decel_medium_cnt = 0

        # [-4, )
        self.DECEL_H_UB = -4
        self.decel_high_cnt = 0

        # [1,2) (-2, -1]
        self.JERK_M_LB_P = 1
        self.JERK_M_UB_P = 2
        self.JERK_M_LB_N = -2
        self.JERK_M_UB_N = -1
        self.jerk_medium_cnt = 0

        # [2, inf) (-inf, -2]
        self.JERK_H_LB_P = 2
        self.JERK_H_UB_N = -2
        self.jerk_high_cnt = 0

        # [1, 2) [-2, -1)
        self.LAT_ACCEL_M_LB_P = 1
        self.LAT_ACCEL_M_UB_P = 2
        self.LAT_ACCEL_M_LB_N = -2
        self.LAT_ACCEL_M_UB_N = -1
        self.lat_accel_medium_cnt = 0

        # [2, inf)  [-inf,-2)
        self.LAT_ACCEL_H_LB_P = 2
        self.LAT_ACCEL_H_UB_N = -2
        self.lat_accel_high_cnt = 0

        # [0.5,1) [-1, -0.5)
        self.LAT_JERK_M_LB_P = 0.5
        self.LAT_JERK_M_UB_P = 1
        self.LAT_JERK_M_LB_N = -1
        self.LAT_JERK_M_UB_N = -0.5
        self.lat_jerk_medium_cnt = 0

        # [1, inf)  [-inf,-1)
        self.LAT_JERK_H_LB_P = 1
        self.LAT_JERK_H_UB_N = -1
        self.lat_jerk_high_cnt = 0

    def put(self, adc_trajectory):
        self.total_cycle_num += 1
        if not self.is_simulation and not self.is_sim:
            latency = adc_trajectory.latency_stats.total_time_ms
            self.module_latency.append(latency)

            self.error_code_analyzer.put(
                adc_trajectory.header.status.error_code)
            self.error_msg_analyzer.put(adc_trajectory.header.status.msg)

            traj_type = planning_pb2.ADCTrajectory.TrajectoryType.Name(
                adc_trajectory.trajectory_type)
            self.trajectory_type_dist[traj_type] = \
                self.trajectory_type_dist.get(traj_type, 0) + 1

            if adc_trajectory.estop.is_estop:
                self.estop_reason_dist[adc_trajectory.estop.reason] = \
                    self.estop_reason_dist.get(
                        adc_trajectory.estop.reason, 0) + 1

        if self.is_simulation or self.is_sim:
            if not adc_trajectory.debug.planning_data.HasField('init_point'):
                return

            init_point = adc_trajectory.debug.planning_data.init_point

            self.init_point_curvature.append(abs(init_point.path_point.kappa))
            self.init_point_dcurvature.append(abs(
                init_point.path_point.dkappa))

            t = adc_trajectory.header.timestamp_sec + init_point.relative_time

            accel = None
            jerk = None
            if self.last_init_point is not None:
                duration = t - self.last_init_point_t
                if duration <= 0:
                    accel = 0
                else:
                    accel = (init_point.v - self.last_init_point.v) / duration
                if accel > 0:
                    self.init_point_accel.append(accel)
                elif accel < 0:
                    self.init_point_decel.append(abs(accel))

                if self.DECEL_M_LB < accel <= self.DECEL_M_UB:
                    self.decel_medium_cnt += 1
                if accel <= self.DECEL_H_UB:
                    self.decel_high_cnt += 1

                if self.ACCEL_M_LB <= accel < self.ACCEL_M_UB:
                    self.accel_medium_cnt += 1
                if self.ACCEL_H_LB <= accel:
                    self.accel_high_cnt += 1

                if self.last_init_point_a is not None:
                    if duration <= 0:
                        jerk = 0
                    else:
                        jerk = (accel - self.last_init_point_a) / duration
                    self.jerk_list.append(abs(jerk))

                    if self.JERK_M_LB_P <= jerk < self.JERK_M_UB_P or \
                        self.JERK_M_LB_N < jerk <= self.JERK_M_UB_N:
                        self.jerk_medium_cnt += 1
                    if jerk >= self.JERK_H_LB_P or jerk <= self.JERK_H_UB_N:
                        self.jerk_high_cnt += 1

                # centripetal_jerk
                centripetal_jerk = 2 * init_point.v * init_point.a \
                    * init_point.path_point.kappa + init_point.v \
                        * init_point.v * init_point.path_point.dkappa
                self.centripetal_jerk_list.append(abs(centripetal_jerk))

                if self.LAT_JERK_M_LB_P <= centripetal_jerk < self.LAT_JERK_M_UB_P \
                    or self.LAT_JERK_M_LB_N < centripetal_jerk <= self.LAT_JERK_M_UB_N:
                    self.lat_jerk_medium_cnt += 1
                if centripetal_jerk >= self.LAT_JERK_H_LB_P \
                    or centripetal_jerk <= self.LAT_JERK_H_UB_N:
                    self.lat_jerk_high_cnt += 1

                # centripetal_accel
                centripetal_accel = init_point.v * init_point.v \
                    * init_point.path_point.kappa
                self.centripetal_accel_list.append(abs(centripetal_accel))

                if self.LAT_ACCEL_M_LB_P <= centripetal_accel < self.LAT_ACCEL_M_UB_P \
                    or self.LAT_ACCEL_M_LB_N < centripetal_accel <= self.LAT_ACCEL_M_UB_N:
                    self.lat_accel_medium_cnt += 1
                if centripetal_accel >= self.LAT_ACCEL_H_LB_P \
                    or centripetal_accel <= self.LAT_ACCEL_H_UB_N:
                    self.lat_accel_high_cnt += 1

            self.last_init_point_t = t
            self.last_init_point = init_point
            self.last_init_point_a = accel

        # TODO(yifei) temporarily disable frechet distance
        #if self.last_adc_trajectory is not None and self.is_simulation:
        #    current_path, last_path = self.find_common_path(adc_trajectory,
        #                                                    self.last_adc_trajectory)
        #    if len(current_path) == 0 or len(last_path) == 0:
        #        dist = 0
        #    else:
        #        dist = frechet_distance(current_path, last_path)
        #        if dist is not None:
        #            self.frechet_distance_list.append(dist)

        self.last_adc_trajectory = adc_trajectory

    def find_common_path(self, current_adc_trajectory, last_adc_trajectory):
        current_path_points = current_adc_trajectory.trajectory_point
        last_path_points = last_adc_trajectory.trajectory_point

        current_path = []
        for point in current_path_points:
            current_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break
        last_path = []
        for point in last_path_points:
            last_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break

        if len(current_path) == 0 or len(last_path) == 0:
            return [], []

        current_ls = LineString(current_path)
        last_ls = LineString(last_path)
        current_start_point = Point(current_path[0])

        dist = last_ls.project(current_start_point)
        cut_lines = self.cut(last_ls, dist)
        if len(cut_lines) == 1:
            return [], []
        last_ls = cut_lines[1]
        dist = current_ls.project(Point(last_path[-1]))
        if dist <= current_ls.length:
            current_ls = self.cut(current_ls, dist)[0]
        else:
            dist = last_ls.project(Point(current_path[-1]))
            last_ls = self.cut(last_ls, dist)[0]
        return current_ls.coords, last_ls.coords

    def cut(self, line, distance):
        if distance <= 0.0 or distance >= line.length:
            return [LineString(line)]
        coords = list(line.coords)
        for i, p in enumerate(coords):
            pd = line.project(Point(p))
            if pd == distance:
                return [LineString(coords[:i + 1]), LineString(coords[i:])]
            if pd > distance:
                cp = line.interpolate(distance)
                return [
                    LineString(coords[:i] + [(cp.x, cp.y)]),
                    LineString([(cp.x, cp.y)] + coords[i:])
                ]

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Planning Latency (ms) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- Planning Trajectroy Type Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.trajectory_type_dist)

        print PrintColors.HEADER + "--- Planning Estop Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.estop_reason_dist)

        print PrintColors.HEADER + "--- Planning Error Code Distribution---" + \
            PrintColors.ENDC
        self.error_code_analyzer.print_results()
        print PrintColors.HEADER + "--- Planning Error Msg Distribution ---" + \
            PrintColors.ENDC
        self.error_msg_analyzer.print_results()

        print PrintColors.HEADER + "--- Planning Trajectory Frechet Distance (m) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(
            self.frechet_distance_list)

    def print_simulation_results(self):
        results = {}

        if len(self.init_point_accel) > 0:
            results["accel_max"] = max(self.init_point_accel)
            results["accel_avg"] = np.average(self.init_point_accel)
        else:
            results["accel_max"] = 0.0
            results["accel_avg"] = 0.0

        results['accel_medium_cnt'] = self.accel_medium_cnt
        results['accel_high_cnt'] = self.accel_high_cnt

        if len(self.init_point_decel) > 0:
            results["decel_max"] = max(self.init_point_decel)
            results["decel_avg"] = np.average(self.init_point_decel)
        else:
            results["decel_max"] = 0.0
            results["decel_avg"] = 0.0

        results['decel_medium_cnt'] = self.decel_medium_cnt
        results['decel_high_cnt'] = self.decel_high_cnt

        if len(self.jerk_list) > 0:
            results["jerk_max"] = max(self.jerk_list, key=abs)
            jerk_avg = np.average(np.absolute(self.jerk_list))
            results["jerk_avg"] = jerk_avg
        else:
            results["jerk_max"] = 0
            results["jerk_avg"] = 0

        results['jerk_medium_cnt'] = self.jerk_medium_cnt
        results['jerk_high_cnt'] = self.jerk_high_cnt

        if len(self.centripetal_jerk_list) > 0:
            results["lat_jerk_max"] = max(self.centripetal_jerk_list, key=abs)
            jerk_avg = np.average(np.absolute(self.centripetal_jerk_list))
            results["lat_jerk_avg"] = jerk_avg
        else:
            results["lat_jerk_max"] = 0
            results["lat_jerk_avg"] = 0

        results['lat_jerk_medium_cnt'] = self.lat_jerk_medium_cnt
        results['lat_jerk_high_cnt'] = self.lat_jerk_high_cnt

        if len(self.centripetal_accel_list) > 0:
            results["lat_accel_max"] = max(self.centripetal_accel_list,
                                           key=abs)
            accel_avg = np.average(np.absolute(self.centripetal_accel_list))
            results["lat_accel_avg"] = accel_avg
        else:
            results["lat_accel_max"] = 0
            results["lat_accel_avg"] = 0

        results['lat_accel_medium_cnt'] = self.lat_accel_medium_cnt
        results['lat_accel_high_cnt'] = self.lat_accel_high_cnt

        print json.dumps(results)

    def print_sim_results(self):
        """
        dreamland metrics for planning v2
        """
        v2_results = {}

        # acceleration
        v2_results["accel"] = {}
        if len(self.init_point_accel) > 0:
            v2_results["accel"]["max"] = max(self.init_point_accel)
            v2_results["accel"]["avg"] = np.average(self.init_point_accel)
        else:
            v2_results["accel"]["max"] = 0.0
            v2_results["accel"]["avg"] = 0.0
        v2_results["accel"]["medium_cnt"] = self.accel_medium_cnt
        v2_results["accel"]["high_cnt"] = self.accel_high_cnt

        # deceleration
        v2_results["decel"] = {}
        if len(self.init_point_decel) > 0:
            v2_results["decel"]["max"] = max(self.init_point_decel)
            v2_results["decel"]["avg"] = np.average(self.init_point_decel)
        else:
            v2_results["decel"]["max"] = 0.0
            v2_results["decel"]["avg"] = 0.0
        v2_results["decel"]["medium_cnt"] = self.decel_medium_cnt
        v2_results["decel"]["high_cnt"] = self.decel_high_cnt

        # jerk
        v2_results["jerk"] = {}
        if len(self.jerk_list) > 0:
            v2_results["jerk"]["max"] = max(self.jerk_list, key=abs)
            jerk_avg = np.average(np.absolute(self.jerk_list))
            v2_results["jerk"]["avg"] = jerk_avg
        else:
            v2_results["jerk"]["max"] = 0
            v2_results["jerk"]["avg"] = 0
        v2_results["jerk"]["medium_cnt"] = self.jerk_medium_cnt
        v2_results["jerk"]["high_cnt"] = self.jerk_high_cnt

        # centripetal_jerk
        v2_results["lat_jerk"] = {}
        if len(self.centripetal_jerk_list) > 0:
            v2_results["lat_jerk"]["max"] = max(self.centripetal_jerk_list,
                                                key=abs)
            jerk_avg = np.average(np.absolute(self.centripetal_jerk_list))
            v2_results["lat_jerk"]["avg"] = jerk_avg
        else:
            v2_results["lat_jerk"]["max"] = 0
            v2_results["lat_jerk"]["avg"] = 0
        v2_results["lat_jerk"]["medium_cnt"] = self.lat_jerk_medium_cnt
        v2_results["lat_jerk"]["high_cnt"] = self.lat_jerk_high_cnt

        # centripetal_accel
        v2_results["lat_accel"] = {}
        if len(self.centripetal_accel_list) > 0:
            v2_results["lat_accel"]["max"] = max(self.centripetal_accel_list,
                                                 key=abs)
            accel_avg = np.average(np.absolute(self.centripetal_accel_list))
            v2_results["lat_accel"]["avg"] = accel_avg
        else:
            v2_results["lat_accel"]["max"] = 0
            v2_results["lat_accel"]["avg"] = 0
        v2_results["lat_accel"]["medium_cnt"] = self.lat_accel_medium_cnt
        v2_results["lat_accel"]["high_cnt"] = self.lat_accel_high_cnt

        print json.dumps(v2_results)

    def plot_path(self, plt, adc_trajectory):
        path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)
        x = []
        y = []
        for point in path_coords:
            x.append(point[0])
            y.append(point[1])
        plt.plot(x, y, 'r-', alpha=0.5)

    def plot_refpath(self, plt, adc_trajectory):
        for path in adc_trajectory.debug.planning_data.path:
            if path.name != 'planning_reference_line':
                continue
            path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)

            ref_path_coord = []
            for point in path.path_point:
                ref_path_coord.append([point.x, point.y])
            ref_path = LineString(ref_path_coord)

            start_point = Point(path_coords[0])
            dist = ref_path.project(start_point)
            ref_path = self.cut(ref_path, dist)[1]

            end_point = Point(path_coords[-1])
            dist = ref_path.project(end_point)
            ref_path = self.cut(ref_path, dist)[0]

            x = []
            y = []
            for point in ref_path.coords:
                x.append(point[0])
                y.append(point[1])

            plt.plot(x, y, 'b--', alpha=0.5)

    def trim_path_by_distance(self, adc_trajectory, s):
        path_coords = []
        path_points = adc_trajectory.trajectory_point
        for point in path_points:
            if point.path_point.s <= s:
                path_coords.append([point.path_point.x, point.path_point.y])
        return path_coords
class PlannigAnalyzer:
    """planning analyzer"""
    def __init__(self, is_simulation):
        """init"""
        self.module_latency = []
        self.trajectory_type_dist = {}
        self.estop_reason_dist = {}
        self.error_code_analyzer = ErrorCodeAnalyzer()
        self.error_msg_analyzer = ErrorMsgAnalyzer()
        self.last_adc_trajectory = None
        self.frechet_distance_list = []
        self.is_simulation = is_simulation
        self.hard_break_list = []
        self.total_cycle_num = 0
        self.init_point_curvature = []
        self.init_point_dcurvature = []

    def put(self, adc_trajectory):
        self.total_cycle_num += 1
        """put"""
        if not self.is_simulation:
            latency = adc_trajectory.latency_stats.total_time_ms
            self.module_latency.append(latency)

            self.error_code_analyzer.put(
                adc_trajectory.header.status.error_code)
            self.error_msg_analyzer.put(adc_trajectory.header.status.msg)

            traj_type = planning_pb2.ADCTrajectory.TrajectoryType.Name(
                adc_trajectory.trajectory_type)
            self.trajectory_type_dist[traj_type] = \
                self.trajectory_type_dist.get(traj_type, 0) + 1

            if adc_trajectory.estop.is_estop:
                self.estop_reason_dist[adc_trajectory.estop.reason] = \
                    self.estop_reason_dist.get(
                        adc_trajectory.estop.reason, 0) + 1

        if self.is_simulation:
            for point in adc_trajectory.trajectory_point:
                if point.a <= -2.0:
                    self.hard_break_list.append(point.a)

            if adc_trajectory.debug.planning_data.HasField('init_point'):
                self.init_point_curvature.append(
                    abs(adc_trajectory.debug.planning_data.init_point.
                        path_point.kappa))
                self.init_point_dcurvature.append(
                    abs(adc_trajectory.debug.planning_data.init_point.
                        path_point.dkappa))

        # TODO(yifei) temporarily disable frechet distance
        #if self.last_adc_trajectory is not None and self.is_simulation:
        #    current_path, last_path = self.find_common_path(adc_trajectory,
        #                                                    self.last_adc_trajectory)
        #    if len(current_path) == 0 or len(last_path) == 0:
        #        dist = 0
        #    else:
        #        dist = frechet_distance(current_path, last_path)
        #        if dist is not None:
        #            self.frechet_distance_list.append(dist)

        self.last_adc_trajectory = adc_trajectory

    def find_common_path(self, current_adc_trajectory, last_adc_trajectory):
        current_path_points = current_adc_trajectory.trajectory_point
        last_path_points = last_adc_trajectory.trajectory_point

        current_path = []
        for point in current_path_points:
            current_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break
        last_path = []
        for point in last_path_points:
            last_path.append([point.path_point.x, point.path_point.y])
            if point.path_point.s > 5.0:
                break

        if len(current_path) == 0 or len(last_path) == 0:
            return [], []

        current_ls = LineString(current_path)
        last_ls = LineString(last_path)
        current_start_point = Point(current_path[0])

        dist = last_ls.project(current_start_point)
        cut_lines = self.cut(last_ls, dist)
        if len(cut_lines) == 1:
            return [], []
        last_ls = cut_lines[1]
        dist = current_ls.project(Point(last_path[-1]))
        if dist <= current_ls.length:
            current_ls = self.cut(current_ls, dist)[0]
        else:
            dist = last_ls.project(Point(current_path[-1]))
            last_ls = self.cut(last_ls, dist)[0]
        return current_ls.coords, last_ls.coords

    def cut(self, line, distance):
        if distance <= 0.0 or distance >= line.length:
            return [LineString(line)]
        coords = list(line.coords)
        for i, p in enumerate(coords):
            pd = line.project(Point(p))
            if pd == distance:
                return [LineString(coords[:i + 1]), LineString(coords[i:])]
            if pd > distance:
                cp = line.interpolate(distance)
                return [
                    LineString(coords[:i] + [(cp.x, cp.y)]),
                    LineString([(cp.x, cp.y)] + coords[i:])
                ]

    def print_latency_statistics(self):
        """print_latency_statistics"""
        print "\n\n"
        print PrintColors.HEADER + "--- Planning Latency (ms) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(self.module_latency)

        print PrintColors.HEADER + "--- Planning Trajectroy Type Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.trajectory_type_dist)

        print PrintColors.HEADER + "--- Planning Estop Distribution" \
                                   " ---" + PrintColors.ENDC
        DistributionAnalyzer().print_distribution_results(
            self.estop_reason_dist)

        print PrintColors.HEADER + "--- Planning Error Code Distribution---" + \
            PrintColors.ENDC
        self.error_code_analyzer.print_results()
        print PrintColors.HEADER + "--- Planning Error Msg Distribution ---" + \
            PrintColors.ENDC
        self.error_msg_analyzer.print_results()

        print PrintColors.HEADER + "--- Planning Trajectory Frechet Distance (m) ---" + \
            PrintColors.ENDC
        StatisticalAnalyzer().print_statistical_results(
            self.frechet_distance_list)

    def print_simulation_results(self):
        results = {}
        # TODO(yifei) temporarily disable frechet distance
        # results['frechet_dist'] = sum(self.frechet_distance_list) /\
        #    len(self.frechet_distance_list)

        results['hard_brake_cycle_num'] = len(self.hard_break_list)

        curvature_99pctl = np.percentile(self.init_point_curvature, 99)
        results['curvature_99pctl'] = curvature_99pctl
        curvature_avg = np.average(self.init_point_curvature)
        results['curvature_avg'] = curvature_avg

        dcurvature_99pctl = np.percentile(self.init_point_dcurvature, 99)
        results['dcurvature_99pctl'] = dcurvature_99pctl
        dcurvature_avg = np.average(self.init_point_dcurvature)
        results['dcurvature_avg'] = dcurvature_avg

        results['overall_score'] = 1 - results['hard_brake_cycle_num'] /\
            float(self.total_cycle_num)
        # TODO(yifei) temporarily disable frechet distance
        #if results['frechet_dist'] > 10:
        #    results['overall_score'] += 0.0
        #else:
        #    results['overall_score'] += (1 - results['frechet_dist'] / 10.0)
        #results['overall_score'] /= 2.0

        print json.dumps(results)

    def plot_path(self, plt, adc_trajectory):
        path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)
        x = []
        y = []
        for point in path_coords:
            x.append(point[0])
            y.append(point[1])
        plt.plot(x, y, 'r-', alpha=0.5)

    def plot_refpath(self, plt, adc_trajectory):
        for path in adc_trajectory.debug.planning_data.path:
            if path.name != 'planning_reference_line':
                continue
            path_coords = self.trim_path_by_distance(adc_trajectory, 5.0)

            ref_path_coord = []
            for point in path.path_point:
                ref_path_coord.append([point.x, point.y])
            ref_path = LineString(ref_path_coord)

            start_point = Point(path_coords[0])
            dist = ref_path.project(start_point)
            ref_path = self.cut(ref_path, dist)[1]

            end_point = Point(path_coords[-1])
            dist = ref_path.project(end_point)
            ref_path = self.cut(ref_path, dist)[0]

            x = []
            y = []
            for point in ref_path.coords:
                x.append(point[0])
                y.append(point[1])

            plt.plot(x, y, 'b--', alpha=0.5)

    def trim_path_by_distance(self, adc_trajectory, s):
        path_coords = []
        path_points = adc_trajectory.trajectory_point
        for point in path_points:
            if point.path_point.s <= s:
                path_coords.append([point.path_point.x, point.path_point.y])
        return path_coords