|
@@ -44,8 +44,8 @@ class DecisionDemo:
|
|
|
nearindex = self.FindNearIndex(tracemap,msg_gpsimu)
|
|
|
# print("near index: ",nearindex)
|
|
|
|
|
|
- for p in xobjarray.obj :
|
|
|
- print(f"x: {p.position.x} y: {p.position.y} ")
|
|
|
+ # for p in xobjarray.obj :
|
|
|
+ # print(f"x: {p.position.x} y: {p.position.y} ")
|
|
|
|
|
|
if nearindex < 0 :
|
|
|
print(" not found near point, return")
|
|
@@ -207,8 +207,8 @@ class DecisionDemo:
|
|
|
|
|
|
# print("ppindex : ",ppindex)
|
|
|
denominator = 2 * localpoints[ppindex].mx *(-1);
|
|
|
- numerator = math.pow(localpoints[ppindex].mx,2) + pow(localpoints[ppindex].my,2);
|
|
|
- fRadius = 1e9;
|
|
|
+ numerator = math.pow(localpoints[ppindex].mx,2) + pow(localpoints[ppindex].my,2)
|
|
|
+ fRadius = 1e9
|
|
|
if math.fabs(denominator)>0:
|
|
|
fRadius = numerator/denominator
|
|
|
else:
|