@@ -390,7 +390,7 @@ def CalcArcLength(self, traj):
390
390
p1 = traj .points [i ]
391
391
dx = p1 .pose .position .x - p0 .pose .position .x
392
392
dy = p1 .pose .position .y - p0 .pose .position .y
393
- ds = np .sqrt (dx ** 2 + dy ** 2 )
393
+ ds = np .sqrt (dx ** 2 + dy ** 2 )
394
394
s_sum += ds
395
395
s_arr .append (s_sum )
396
396
return s_arr
@@ -408,7 +408,7 @@ def CalcArcLengthPathWLid(self, traj):
408
408
p1 = traj .points [i ].point
409
409
dx = p1 .pose .position .x - p0 .pose .position .x
410
410
dy = p1 .pose .position .y - p0 .pose .position .y
411
- ds = np .sqrt (dx ** 2 + dy ** 2 )
411
+ ds = np .sqrt (dx ** 2 + dy ** 2 )
412
412
s_sum += ds
413
413
s_arr .append (s_sum )
414
414
return s_arr
@@ -426,7 +426,7 @@ def CalcArcLengthPath(self, traj):
426
426
p1 = traj .points [i ]
427
427
dx = p1 .pose .position .x - p0 .pose .position .x
428
428
dy = p1 .pose .position .y - p0 .pose .position .y
429
- ds = np .sqrt (dx ** 2 + dy ** 2 )
429
+ ds = np .sqrt (dx ** 2 + dy ** 2 )
430
430
s_sum += ds
431
431
s_arr .append (s_sum )
432
432
return s_arr
@@ -459,7 +459,7 @@ def CalcAcceleration(self, traj):
459
459
v = 0.5 * (v1 + v0 )
460
460
dx = p1 .pose .position .x - p0 .pose .position .x
461
461
dy = p1 .pose .position .y - p0 .pose .position .y
462
- ds = np .sqrt (dx ** 2 + dy ** 2 )
462
+ ds = np .sqrt (dx ** 2 + dy ** 2 )
463
463
dt = ds / max (abs (v ), 0.001 )
464
464
a = (v1 - v0 ) / dt
465
465
a_arr .append (a )
@@ -480,11 +480,11 @@ def CalcJerk(self, traj):
480
480
481
481
dx0 = p1 .pose .position .x - p0 .pose .position .x
482
482
dy0 = p1 .pose .position .y - p0 .pose .position .y
483
- ds0 = np .sqrt (dx0 ** 2 + dy0 ** 2 )
483
+ ds0 = np .sqrt (dx0 ** 2 + dy0 ** 2 )
484
484
485
485
dx1 = p2 .pose .position .x - p1 .pose .position .x
486
486
dy1 = p2 .pose .position .y - p1 .pose .position .y
487
- ds1 = np .sqrt (dx1 ** 2 + dy1 ** 2 )
487
+ ds1 = np .sqrt (dx1 ** 2 + dy1 ** 2 )
488
488
489
489
dt0 = ds0 / max (abs (0.5 * (v1 + v0 )), 0.001 )
490
490
dt1 = ds1 / max (abs (0.5 * (v2 + v1 )), 0.001 )
0 commit comments