Relative distance of Lead Vehicle

In this notebook, we will show how to retreive lead vehicle’s information, analyze them and denoise them

import signal
import pandas as pd
import sys, math, time, datetime
import matplotlib.pyplot as plt
import numpy as np
import pickle
from strym import strymread
import strym
from datetime import datetime

datafolder = "../../PandaData/2020_03_03/"
import glob
csvlist = glob.glob(datafolder+"*.csv")
csvlist

['../../PandaData/2020_03_03/2020-03-03-15-36-24-479038__CAN_Messages.csv',
 '../../PandaData/2020_03_03/2020-03-03-18-29-15-641040__CAN_Messages.csv',
 '../../PandaData/2020_03_03/2020-03-03-18-09-36-362663__CAN_Messages.csv',
 '../../PandaData/2020_03_03/2020-03-03-15-27-20-702814__CAN_Messages.csv',
 '../../PandaData/2020_03_03/2020-03-03-19-57-11-286348__CAN_Messages.csv',
 '../../PandaData/2020_03_03/2020-03-03-19-32-39-704415__CAN_Messages.csv',
 '../../PandaData/2020_03_03/2020-03-03-10-35-13-966077__CAN_Messages.csv',
 '../../PandaData/2020_03_03/2020-03-03-20-15-01-595829__CAN_Messages.csv']

dbcfile = '../examples/newToyotacode.dbc'
r0 = strymread(csvfile=csvlist[1], dbcfile=dbcfile)

lead_distance = r0.lead_distance()
strymread.plt_ts(lead_distance, title="Lead Vehicle's Distance [m]")

Now lets filter out lead distance for when RADAR actually saw an object in the front.

A value of 252 means, RADAR didn’t see anything in the front

lead_distance_filtered = lead_distance[lead_distance['Message'] < 252]
strymread.plt_ts(lead_distance_filtered, "Filtered Lead Distance when\nRADAR actually saw something in the front")

Working with RADAR traces

Let’s directly work with RADAR traces. Toyota RAV4 has 16 different tracks of RADAR traces. We will get them all

track_a_long = r0.long_dist(np.arange(0, 16))
track_a_lat = r0.lat_dist(np.arange(0, 16))

track_a_long and track_a_lat are list of timeseries dataframe each representing a differen track. Lets plot all.

Visualize Radar traces

for i in range(0, 16):
    fig, ax = strymread.create_fig(2)
    ax[0].scatter(x = track_a_long[i]['Message'],y  = track_a_lat[i]['Message'], s = 2, c= track_a_long[i]['Time'])
    ax[0].set(xlabel='Longitudinal Distance', ylabel='Lateral Distance')
    ax[0].set_title('Track A ' + str(i))

    ax[1].scatter(x = 'Time',y  = 'Message' , data = track_a_long[i], s = 2)
    ax[1].set(xlabel='Time', ylabel='Longitudinal Distance')
    ax[1].set_title('Track A ' + str(i))

    plt.show()

np.max(track_a_long[i]['Message'])

327.64

Keep indices where distance longitudinal distance < 100

indices_less_100 = track_a_long[i]['Message'] < 100

track_a_long_100 = track_a_long[i][indices_less_100]
track_a_lat_100 = track_a_lat[i][indices_less_100]

fig, ax = strymread.create_fig(2)
ax[0].scatter(x = track_a_long_100['Message'], y = track_a_lat_100['Message'],c = track_a_long_100['Time'], s = 1)
ax[0].set(ylabel='Lateral Distance', xlabel='Longitudinal Distance')
ax[1].scatter(x = track_a_long_100['Time'], y = track_a_long_100['Message'], c = track_a_long_100['Time'], s = 1)
ax[1].set(xlabel='Time', ylabel='Longitudinal Distance')
plt.show()

Now lets remove lateral distance greater 1.0 m on both side of the car.

indices_less_abs1 = np.abs(track_a_lat_100['Message']) <= 1.0

track_a_long_abs1 = track_a_long_100[indices_less_abs1]
track_a_lat_abs1 = track_a_lat_100[indices_less_abs1]

fig, ax = strymread.create_fig(2)
ax[0].scatter(x = track_a_long_abs1['Message'], y = track_a_lat_abs1['Message'],c = track_a_long_abs1['Time'], s = 1)
ax[0].set(ylabel='Lateral Distance', xlabel='Longitudinal Distance')
ax[1].scatter(x = track_a_long_abs1['Time'], y = track_a_long_abs1['Message'], c = track_a_long_abs1['Time'], s = 1)
ax[1].set(xlabel='Time', ylabel='Longitudinal Distance')
plt.show()

strymread.plt_ts(track_a_long_abs1, "Filtered Radar traces of 'A' Track")
strymread.plt_ts(lead_distance_filtered, "Filtered Lead distance")

We can further filter out RADAR trace to just look within cone of a certain degree

indices_within_cone = []
for index, row in track_a_long_abs1.iterrows():
    if track_a_lat_abs1['Message'][index] == 0.0:
        continue
    if np.arctan(track_a_long_abs1['Message'][index]/track_a_lat_abs1['Message'][index]) <= 0.6:
        indices_within_cone.append(index)

long_within_cone = track_a_long_abs1.loc[indices_within_cone]
lat_within_cone = track_a_lat_abs1.loc[indices_within_cone]

strymread.plt_ts(long_within_cone, "Filtered Radar traces of 'A' Track (cone filter added)")
strymread.plt_ts(lead_distance_filtered, "Filtered Lead distance")

Lets’s write a function to perform all filtering at once

def filter_radar(long_ts, lat_ts, long_threshold = 100.0, lat_threshold = 1.0, cone_threshold = 0.6):
    # filter based on longitudinal distance threshold
    indices_less_100 = long_ts['Message'] < long_threshold
    track_a_long_100 = long_ts[indices_less_100]
    track_a_lat_100 = lat_ts[indices_less_100]

    # filter based on lateral distance threshold
    indices_less_abs1 = np.abs(track_a_lat_100['Message']) <= lat_threshold
    track_a_long_abs1 = track_a_long_100[indices_less_abs1]
    track_a_lat_abs1 = track_a_lat_100[indices_less_abs1]

    # filter based on cone
    indices_within_cone = []
    for index, row in track_a_long_abs1.iterrows():
        if track_a_lat_abs1['Message'][index] == 0.0:
            continue
        if np.arctan(track_a_long_abs1['Message'][index]/track_a_lat_abs1['Message'][index]) <= cone_threshold:
            indices_within_cone.append(index)

    long_within_cone = track_a_long_abs1.loc[indices_within_cone]
    lat_within_cone = track_a_lat_abs1.loc[indices_within_cone]

    return long_within_cone, lat_within_cone

lo, lt = filter_radar(track_a_long[i], track_a_lat[i])

strymread.plt_ts(lo, "Filtered Radar traces of 'A' Track (cone filter added)")

Now lets combine above approach for all the TRACKS

dflong = pd.concat(track_a_long)
dflat = pd.concat(track_a_lat)

dflong.sort_index(inplace=True)
dflat.sort_index(inplace=True)

longdf, latdf = filter_radar(dflong, dflat, cone_threshold = 0.05)

strymread.plt_ts(longdf, "Filtered Longitudinal RADAR Data from Track A ALL IDS")
strymread.plt_ts(lead_distance_filtered, "Filtered Lead distance")

We can further denosie the filtered longitudinal radar trace

denoisedDF = strymread.denoise(longdf, window_size=20)

strymread.plt_ts(denoisedDF, "Denoised Filtered Longitudinal RADAR Data from Track A ALL IDS")
strymread.plt_ts(lead_distance_filtered, "Filtered Lead distance")

In conclusion, we see that working with RADAR traces directly involves a lot of filtering and denoising to get some information that might be available on other Message IDs in cleaner form. Nevertheless, we might be able to uncover some other information when directly working with RADAR traces that might not be available otherwise.