def easy_test(path):
current_position = easyVector.get_poseXY()
if len(path) == 0 or FlagMORP:
path = get_pathlist()
i = 0
if i == len(path) - 1:
print('Goal!')
else:
if (path[0][i] - current_position[0])**2 + (
path[1][i] - current_position[1])**2 < threshold_boundary**2:
i = i + 1
easy_drive(path[0][i], path[1][i], current_position)
def dap(path, velRobot=0.1, verbose=0): # path is 2D array (x, y)
global start_position, init_path
init_path = path
global i, threshold_boundary
while (True):
print('thread re1fuck')
if rospy.get_param('/point_init'):
#closest_pt()
rospy.set_param('/point_init', False)
if i == 0 or FlagMORP: # when just running code or we should search new path(by MORP algorithm)
while (True): # at first time, get_poseXY() == (0, 0)
start_position = easyVector.get_poseXY(
)[:] # x axis == forward
#print(start_position, easyVector.get_angle())
if start_position != (0.0, 0.0):
break
if verbose: # plot current robot position
fig, ax = plt.subplots()
threshold_boundary = 0.2 #0.14
_x = [item[0] for item in path]
_y = [item[1] for item in path]
plt.scatter(_x, _y, color='b')
plt.hold(True)
plt.xlim(-1, 1.3)
plt.ylim(-2, 2)
plt.hold(True)
current_position = get_current_pose(start_position)
#(x, y) when DPoom head to right, x decreases (negatively increases)
# current_position and DAP's coordinates : straight(y), right(x)
#print('current_position :', current_position)
if (path[i][0] - current_position[0])**2 + (
path[i][1] - current_position[1])**2 < threshold_boundary**2:
i = i + 1
if i == len(path):
print('Goal!')
easyGo.stop()
if verbose:
plt.close()
break
print('i:', i)
if cv2.waitKey(1) == ord('k'):
easyGo.stop()
exit()
break
easy_drive(path[i][0], path[i][1], current_position, velRobot)
if verbose:
plt.scatter(current_position[0], current_position[1], c='r')
plt.hold(True)
plt.pause(0.05)
def get_current_pose(start_position):
return((-(easyVector.get_poseXY()[1] - start_position[1]),
easyVector.get_poseXY()[0] - start_position[0]))