def GPSTxT_leader(self):
global distance_difference, present_num, search, b, now_po
base = base_frame()
node_index = Int32()
while not rospy.is_shutdown():
my_x, my_y = self.listener()
path_leng = 0
search = 0
a = 0
a_grad = 0
distance_difference = 100
while (abs(distance_difference) > 0.01):
distance_difference = (my_x - p[0][present_num + 1])**2 + (
my_y - p[1][present_num + 1])**2 - (
my_x - p[0][present_num])**2 - (my_y -
p[1][present_num])**2
search = search + 1
a = -distance_difference / (1 + search / 100)
if (a > -1 and a < 0):
a = -1
elif (a < 1 and a > 0):
a = 1
present_num = present_num + int(a)
if present_num > p_length - 2:
present_num = p_length - 2
break
elif present_num < 0:
present_num = 0
break
elif (search == 10):
break
plt.plot(my_x, my_y, 'ro', label='position')
Fsum = 0
s = [0]
while (Fsum < 10):
big_node = int((present_num + path_leng) / Nsample)
if (big_node > leng - 1):
break
ft = lambda t: (
(3 * poly[0][big_node][1] * t * t + 2 * poly[1][big_node][
1] * t + poly[2][big_node][1])**2 +
(3 * poly[0][big_node][0] * t * t + 2 * poly[1][big_node][
0] * t + poly[2][big_node][0])**2)**0.5
for small_node in range(0, Nsample):
F, err = quad(ft,
float(small_node) / Nsample,
float(small_node + 1) / Nsample)
Fsum = Fsum + F
path_leng = path_leng + 1
if Fsum > 10:
break
elif present_num + path_leng > p_length - 1:
break
s.append(Fsum)
#print(len(s),path_leng)
#s=[Fsum]
if (len(s) > 2):
distance = ((my_x - p[0][present_num])**2 +
(my_y - p[1][present_num])**2)**0.5
fpx = np.poly1d(
np.polyfit(s, p[0][present_num:present_num + path_leng],
3))
fpy = np.poly1d(
np.polyfit(s, p[1][present_num:present_num + path_leng],
3))
ix = fpx([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10])
iy = fpy([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10])
iangle = []
idistance = []
for scurve in range(0, Nsample):
iangle.append(
math.atan2(iy[scurve + 1] - iy[scurve],
ix[scurve + 1] - ix[scurve]) * 180 / PI)
idistance.append(
math.sqrt((iy[scurve + 1] - iy[scurve])**2 +
(ix[scurve + 1] - ix[scurve])**2))
a = ix[10] - ix[0]
b = iy[10] - iy[0]
c = my_x - ix[0]
d = my_y - iy[0]
if (a * d - b * c) > 0:
direction = 1
else:
direction = -1
now_po = Point()
now_msg = Marker(
type=Marker.POINTS,
# points=Point(bm[i][0],bm[i][1],0),
lifetime=rospy.Duration(0),
scale=Vector3(0.5, 0.5, 0.1),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8))
now_po.x = p[0][present_num] - self.offset_X
now_po.y = p[1][present_num] - self.offset_Y
now_po.z = 0
now_msg.points = [now_po]
now_msg.id = 6
# rviz_msg.points.x=p.x
self.now_plot.publish(now_msg)
print("------------------------------")
print(direction, distance)
print(present_num, search, s[len(s) - 1])
print(my_x, my_y)
print("------------------------------")
# print(ix) print(iy) print(iangle) print(idistance)
# now_po.x=my_x-self.offset_X
# now_po.y=my_y-self.offset_Y
# now_po.z=0
# now_po.points=[po]
# now_po.id=0
# rviz_msg.points.x=p.x
# self.now_plot.publish(now_po)
node_index.data = int(present_num / Nsample)
base.distance = direction * distance
base.s_x = ix
base.s_y = iy
base.s_a = iangle
base.s_d = idistance
base.tm_x = my_x
base.tm_y = my_y
self.big_node_num.publish(node_index)
self.base_frame.publish(base)
def GPSTxT_leader(self):
global distance_difference, present_num, search
base = base_frame()
while not rospy.is_shutdown():
my_x, my_y = self.listener()
path_leng = 0
search = 0
a = 0
a_grad = 0
distance_difference = 100
while (abs(distance_difference) > 0.05):
if present_num > p_length - 1:
present_num = Nsample * (leng - 1)
elif present_num < 0:
present_num = 0
distance_difference = (my_x - p[0][present_num + 1])**2 + (
my_y - p[1][present_num + 1])**2 - (
my_x - p[0][present_num])**2 - (my_y -
p[1][present_num])**2
search = search + 1
a = -distance_difference / (1 + search / 100)
if (a > -1 and a < 0):
a = -1
elif (a < 1 and a > 0):
a = 1
present_num = present_num + int(a)
#print(search,present_num,distance_difference,a)
if (search == 100):
print("binzino")
break
# plt.plot(my_x,my_y, 'ro', label='position')
Fsum = 0
st = [0]
s = [0]
while (Fsum < 10):
big_node = int((present_num + path_leng) / Nsample)
ft = lambda t: (
(3 * poly[0][big_node][1] * t * t + 2 * poly[1][big_node][
1] * t + poly[2][big_node][1])**2 +
(3 * poly[0][big_node][0] * t * t + 2 * poly[1][big_node][
0] * t + poly[2][big_node][0])**2)**0.5
for small_node in range(0, Nsample):
F, err = quad(ft,
float(small_node) / Nsample,
float(small_node + 1) / Nsample)
Fsum = Fsum + F
path_leng = path_leng + 1
if Fsum > 10:
break
elif (present_num + path_leng) > (Nsample * (leng - 1)):
break
s.append(Fsum)
#s=[Fsum]
distance = ((my_x - p[0][present_num])**2 +
(my_y - p[1][present_num])**2)**0.5
fpx = np.poly1d(
np.polyfit(s, p[0][present_num:present_num + path_leng], 3))
fpy = np.poly1d(
np.polyfit(s, p[1][present_num:present_num + path_leng], 3))
ix = fpx([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10])
iy = fpy([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10])
iangle = []
idistance = []
for scurve in range(0, Nsample):
iangle.append(
math.atan2(iy[scurve + 1] - iy[scurve],
ix[scurve + 1] - ix[scurve]) * 180 / PI)
idistance.append(
math.sqrt((iy[scurve + 1] - iy[scurve])**2 +
(ix[scurve + 1] - ix[scurve])**2))
print(distance)
print(ix)
print(iy)
print(iangle)
print(present_num)
print(idistance)
base.distance = distance
base.s_x = ix
base.s_y = iy
base.s_a = iangle
base.s_d = idistance
self.base_frame.publish(base)