I investigated how to use pandas with python, so that memo. I've just started implementing it in python, so I have a reminder of how to write python.
What you are doing in the program. Get the value to compare with the true value. Obtain the vector of each time axis and check the difference.
Program from here The usage environment is python3
"""
import pandas as pd
import numpy as np
import math
class Trajectory:
def __init__(self, csvData: pd.DataFrame):
self.csvData = csvData
self.true_x = self.csvData['true_x[m]']
self.true_y = self.csvData["true_y[m]"]
self.true_theta = self.csvData["true_theta[deg]"]
self.tra_x = self.csvData["trajectory_x[m]"]
self.tra_y = self.csvData["trajectory_y[m]"]
self.tra_theta = self.csvData["trajectory_theta[deg]"]
self.time = self.csvData["time"]
def calcRPEData(self):
true_length = self.__calcVector(self.true_x,self.true_y)
tra_length = self.__calcVector(self.tra_x,self.tra_y)
self.RPEDataLength = pd.Series(np.array(tra_length) - np.array(true_length))
theta = np.diff(self.true_theta.values)
theta = np.insert(theta, 0, 0)
#180 because theta is 0 to 180 degrees->Prevents a big change when it reaches 0
theta = np.where(theta > 180 , theta -180, theta)
theta = np.where(theta < -180 , theta +180, theta)
self.RPEDataTheta = pd.Series(theta)
self.csvData["RPE_LengthError[m]"] = self.RPEDataLength
self.csvData["RPE_ThetaError[deg]"] = self.RPEDataTheta
self.csvData["RPE_LE_Abs[m]"] = self.RPEDataLength.abs()
self.csvData["RPE_TE_Abs[m]"] = self.RPEDataTheta.abs()
return self.RPEDataLength, self.RPEDataTheta
def calcObjectError(self):
#Length error amount+LRF distance x tan(theta error)
lengthError = np.array(self.RPEDataLength)
thetaError = []
for theta in self.RPEDataTheta:
thetaError.append( 30 * math.tan(math.radians(theta)))
thetaError = np.array(thetaError)
objectError = np.abs(lengthError) + np.abs(thetaError)
self.objectError = pd.Series(objectError)
self.csvData["RPE_ObjectError[m]"] = self.objectError
return self.objectError
def __calcVector(self,x :pd.Series, y :pd.Series) -> pd.Series:
length = []
#Array length-Run once
length.append(0)
for colmun in range( len(x)-1 ):
length.append( ((x[colmun + 1] -x[colmun]) ** 2 + (y[colmun +1] - y[colmun +1]) ** 2 )**(1/2))
return length
def add_tag(self):
#Acceleration / deceleration judgment
self.csvData["straightState"] = self.__judgeStraight()
self.csvData["curveState"] = self.__judgeCurve()
def __calcVelAndAccel(self):
#Judgment by true value
#Distance calculation
length = self.__calcVector(self.true_x,self.true_y)
#Calculation of speed Travel distance/Convert to time np type
vel = np.array(length) / self.time.values
#Calculation of acceleration Amount of change in speed/time
#Speed calculation-->This time speed-Last time speed/This time-Last time
vel_dif = np.diff(vel)
#print(true_vel)
#print(true_vel_dif)
time_dif = np.diff(np.array(self.time))
#print(self.time)
#print(time_dif)
accel = vel_dif / time_dif
accel = np.insert(accel, 0, 0)
return vel,accel
def __judgeStraight(self):
vel,accel = self.__calcVelAndAccel()
#acceleration>0 ->acceleration
#acceleration<0 ->Decelerate
#Speed is 0 and speed is 0, stop STOP
#other than that->Constant speed
straightState = []
print(vel)
print(accel)
for colmun, item in enumerate(vel):
if accel[colmun] > 0:
state = "S_Accel"
elif accel[colmun] < 0:
state = "S_Decel"
elif vel[colmun] == 0:
state = "S_Stop"
else:
state = "S_Constant"
straightState.append(state)
return straightState
def __judgeCurve(self):
vel, accel = self.__calcAngleVelAndAccel()
#acceleration>0 ->acceleration
#acceleration<0 ->Decelerate
#Speed is 0 and speed is 0, stop STOP
#other than that->Constant speed
State = []
print(vel)
print(accel)
for colmun, item in enumerate(vel):
if accel[colmun] > 0:
state = "C_Accel"
elif accel[colmun] < 0:
state = "C_Decel"
elif vel[colmun] == 0:
state = "C_Stop"
else:
state = "C_Constant"
State.append(state)
return State
def __calcAngleVelAndAccel(self):
#Judgment by true value
#Distance calculation
theta = np.diff(self.true_theta.values)
theta = np.insert(theta, 0, 0)
#Calculation of speed Travel distance/Convert to time np type
vel = theta / self.time.values
#Calculation of acceleration Amount of change in speed/time
#Speed calculation-->This time speed-Last time speed/This time-Last time
vel_dif = np.diff(vel)
#print(true_vel)
#print(true_vel_dif)
time_dif = np.diff(np.array(self.time))
#print(self.time)
#print(time_dif)
accel = vel_dif / time_dif
accel = np.insert(accel, 0, 0)
return vel,accel
import pandas as pd
import trajectpryEdditer
from pathlib import Path
#Create Path object
p = Path("./true_trajectory")
#Search for csv files
list(p.glob("**/*.csv"))
trajectory = []
for csv in list(p.glob("**/*.csv")):
trajectoryData =pd.read_csv(csv)
trajectory.append(trajectpryEdditer.Trajectory(trajectoryData))
trajectory[0].add_tag()
a = trajectory[0].csvData[(trajectory[0].csvData['straightState'] == 'S_Accel') & (trajectory[0].csvData['curveState'] == 'C_Decel') ]
trajectory[0].calcRPEData()
trajectory[0].calcObjectError()
Recommended Posts