J'ai étudié comment utiliser les pandas avec python, donc ce mémo. Depuis que je viens de commencer à l'implémenter en python, il existe également un mémorandum sur la façon d'écrire en python.
Ce que vous faites dans le programme. Obtenez la valeur à comparer avec la vraie valeur. Obtenez le vecteur de chaque axe temporel et vérifiez la différence.
Programme d'ici L'environnement d'utilisation est 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 car thêta est de 0 à 180 degrés->Empêche un grand changement quand il atteint 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):
#Montant de l'erreur de longueur+Distance LRF x tan(erreur thêta)
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 = []
#Longueur de séquence-Exécuter une fois
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):
#Jugement d'accélération / décélération
self.csvData["straightState"] = self.__judgeStraight()
self.csvData["curveState"] = self.__judgeCurve()
def __calcVelAndAccel(self):
#Jugement par la vraie valeur
#Calcul de la distance
length = self.__calcVector(self.true_x,self.true_y)
#Calcul de la vitesse Distance parcourue/Convertir en type np temporel
vel = np.array(length) / self.time.values
#Calcul de l'accélération Quantité de changement de vitesse/temps
#Calcul de la vitesse-->Cette vitesse de temps-Vitesse de la dernière fois/Cette fois-Dernière fois
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()
#accélération>0 ->accélération
#accélération<0 ->Ralentir
#La vitesse est 0 et la vitesse est 0, arrêter STOP
#autre que ça->Vitesse constante
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()
#accélération>0 ->accélération
#accélération<0 ->Ralentir
#La vitesse est 0 et la vitesse est 0, arrêter STOP
#autre que ça->Vitesse constante
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):
#Jugement par la vraie valeur
#Calcul de la distance
theta = np.diff(self.true_theta.values)
theta = np.insert(theta, 0, 0)
#Calcul de la vitesse Distance parcourue/Convertir en type np temporel
vel = theta / self.time.values
#Calcul de l'accélération Quantité de changement de vitesse/temps
#Calcul de la vitesse-->Cette vitesse de temps-Vitesse de la dernière fois/Cette fois-Dernière fois
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
#Créer un objet Path
p = Path("./true_trajectory")
#Rechercher des fichiers csv
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