def send_velocity():
global pd0, pd, state, state2start, startangle, ready_to_write
global imuYaw
global state, esc_pd
global count_state_0
global X
x, y, theta = X
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
if time.time() - globalstarttime > 40 and ready_to_write:
state = 2
ready_to_write = False
if state == 0: # try to reach starting angle
velocity.angular.z = min(
max(pd_turn.pd_out(imuYaw), -angular_velocity_limit),
angular_velocity_limit)
if abs(velocity.angular.z) < 0.5:
velocity.angular.z = 0
velocity.linear.x = 0
if abs(linemath.norm(imuYaw - startangle)) < 0.17:
count_state_0 += 1
if count_state_0 >= 5:
velocity.angular.z = 0
state = 1
elif state == 1: # find a point on the curve to set heading to
try:
fhandle.write("{};{} {}\n".format(
x, y, ((x - end[0])**2 + (y - end[1])**2)**(1 / 2.)))
except:
pass
begin_time = time.time()
if ((x - end[0])**2 + (y - end[1])**2)**(
1 / 2.) < 0.05: # if distance to goal is less than 5cm, stop
state = 2
m = 1000 # distance of the curve starting at high value
ind = 0
for i in range(len(cur)): # find closest curve point
if curvemath.my_dist(cur[i], (x, y)) < m:
m = curvemath.my_dist(cur[i], (x, y))
ind = i
temp = int(0.1 * resolution / size) # how many samples to look forward
if ind + temp < len(cur): # if curve finished, pick end point
p1 = cur[ind + temp]
else:
p1 = cur[-1]
angle_setpoint = linemath.norm(np.arctan2(p1[1] - y,
p1[0] - x)) #find setpoint
angle_setpoint += 2 * np.pi # may be obsolete
pd_turn.setpoint = angle_setpoint
temp = pd_turn.pd_out(
linemath.norm(imuYaw + startang_robot) +
2 * np.pi) # take PD output
w = min(max(temp, -angular_velocity_limit),
angular_velocity_limit) # restrain it
velocity.angular.z = w
velocity.linear.x = 0.2
elif state == 2: #plotting the results for future debugging
ready_to_write = False
velocity.linear.x = 0
velocity.angular.z = 0
velocity_pub.publish(velocity)
fhandle.close()
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info",
'r') as fhandle2:
z = fhandle2.read().split('\n')[:-1]
x1 = [float(i.split(' ')[0].split(';')[0]) for i in z]
y1 = [float(i.split(' ')[0].split(';')[1]) for i in z]
plt.plot([i[0] for i in obs], [i[1] for i in obs], 'o')
plt.plot([i[0] for i in obs_s], [i[1] for i in obs_s], 'o')
plt.plot([i[0] for i in cur], [i[1] for i in cur], 'o')
plt.plot(x1, y1, '--', linewidth=2)
plt.title("C: {}".format(atan_coefficient))
plt.xlim(-0.75, 0.75)
plt.ylim(-0.75, 0.75)
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'r') as fhandle2:
fignum = int(fhandle2.read().strip())
print fignum
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'w') as fhandle2:
fhandle2.write("{}\n".format(fignum + 1))
plt.savefig(
"/home/ubuntu/catkin_ws/src/alpha_star/scripts/astartest{}.png".
format(fignum))
state = 3
velocity_pub.publish(velocity)
obstacles = [((0.30, 0.15), (-0.30, -0.15))] #obstacles
resolution = 80 # grid resolution
size = 1.5 # square map size
safety_net = 0.13 # distance from obstacles
cur, obs, obs_s = curvemath.find_curve(
start=start,
end=end,
resolution=resolution,
safety_net=safety_net,
obstacles=obstacles,
smoothing_range=0.8,
plot_ret=True) # curvemath returns the curve
startang_curve = np.arctan2(cur[1][1] - cur[0][1],
cur[1][0] - cur[0][0]) # find the starting angle
startangle = linemath.norm(startang_curve - startang_robot)
fhandle = file("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info",
'w') # for debugging and plotting
pd_turn = pdcon.pd_controller(setpoint=startangle,
kp=5,
kd=0.4,
f=linemath.norm)
esc_pd = False
def send_velocity():
global pd0, pd, state, state2start, startangle, ready_to_write
global imuYaw
global state, esc_pd
global count_state_0
global X
def send_velocity():
global sonarF_val
global sonarFL_val
global sonarFR_val
global sonarL_val
global sonarR_val
global pd0, pd, state, substate, state2start, startangle, ready_to_write
global sonarF, sonarL, sonarFL
global imuYaw
global state
global count_state_0
global X
x, y, theta = X
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
if time.time() - globalstarttime > 40 and ready_to_write:
state = 2
ready_to_write = False
if state == 0:
#fhandle.write("theta: {}\n".format(theta))
#fhandle.write("imuyaw {}\n".format(imuYaw))
velocity.angular.z = -min(max(pd_turn.pd_out(imuYaw)[0], -2.8), 2.8)
velocity.linear.x = 0
#fhandle.write("diafora {}\n".format(linemath.norm(imuYaw - startangle)))
if abs(linemath.norm(imuYaw - startangle)) < 0.17:
count_state_0 += 1
if count_state_0 >= 5:
velocity.angular.z = 0
state = 1
elif state == 1:
try:
fhandle.write("{};{} {} ".format(
x, y, ((x - end[0])**2 + (y - end[1])**2)**(1 / 2.)))
except:
pass
begin_time = time.time()
if ((x - end[0])**2 + (y - end[1])**2)**(1 / 2.) < 0.12:
state = 2
m = 1000
ind = 0
for i in range(len(curve)):
if my_dist(cur[i], (x, y)) < m:
m = my_dist(cur[i], (x, y))
ind = i
if ind == 0:
p1 = cur[ind]
p2 = cur[ind + 1]
else:
p2 = cur[ind]
p1 = cur[ind - 1]
if linemath.is_it_left(p1, p2, (x, y)):
m = -m
temp = pd.pd_out(m)[0]
w = min(max(temp, -angular_velocity_limit), angular_velocity_limit)
points_ahead = int(curve_lookahead / size * resolution)
if points_ahead + ind + 1 < len(cur):
p3 = cur[ind + points_ahead]
p4 = cur[ind + points_ahead + 1]
a1 = np.arctan2(p2[1] - p1[1], p2[0] - p1[0])
a2 = np.arctan2(p4[1] - p3[1], p4[0] - p3[0])
diffa = linemath.norm(a2 - a1)
angout, kpa, kda = pda.pd_out(diffa)
w += angout
fhandle.write("pda_out: {} kpa: {} kda: {}\n".format(angout, kpa, kda))
velocity.angular.z = min(max(w, -angular_velocity_limit),
angular_velocity_limit)
velocity.linear.x = 0.2
elif state == 2:
ready_to_write = False
velocity.linear.x = 0
velocity.angular.z = 0
velocity_pub.publish(velocity)
fhandle.close()
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info",
'r') as fhandle2:
z = fhandle2.read().split('\n')[:-1]
x1 = [float(i.split(' ')[0].split(';')[0]) for i in z]
y1 = [float(i.split(' ')[0].split(';')[1]) for i in z]
plt.plot([i[0] for i in obs], [i[1] for i in obs], 'o')
plt.plot([i[0] for i in cur], [i[1] for i in cur], 'o')
plt.plot(x1, y1, 'o')
plt.title("kp: {}, kd: {}, kpa: {}, kda: {}".format(
pd.kp, pd.kd, pda.kp, pda.kd))
plt.xlim(-0.75, 0.75)
plt.ylim(-0.75, 0.75)
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'r') as fhandle2:
fignum = int(fhandle2.read().strip())
print fignum
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'w') as fhandle2:
fhandle2.write("{}\n".format(fignum + 1))
plt.savefig(
"/home/ubuntu/catkin_ws/src/alpha_star/scripts/astartest{}.png".
format(fignum))
state = 3
velocity_pub.publish(velocity)
def send_velocity():
global pd0, pd, state, substate, state2start, startangle, ready_to_write
global imuYaw
global state
global count_state_0
global X
x, y, theta = X
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
if time.time() - globalstarttime > 40 and ready_to_write:
state = 2
ready_to_write = False
if state == 0:
#fhandle.write("theta: {}\n".format(theta))
#fhandle.write("imuyaw {}\n".format(imuYaw))
velocity.angular.z = min(max(pd_turn.pd_out(imuYaw), -2.8), 2.8)
if abs(velocity.angular.z) < 0.5:
velocity.angular.z = 0
velocity.linear.x = 0
#fhandle.write("diafora {}\n".format(linemath.norm(imuYaw - startangle)))
if abs(linemath.norm(imuYaw - startangle)) < 0.17:
count_state_0 += 1
if count_state_0 >= 5:
velocity.angular.z = 0
state = 1
elif state == 1:
try:
fhandle.write("{};{} {}\n".format(
x, y, ((x - end[0])**2 + (y - end[1])**2)**(1 / 2.)))
except:
pass
begin_time = time.time()
if ((x - end[0])**2 + (y - end[1])**2)**(1 / 2.) < 0.05:
state = 2
m = 1000
ind = 0
for i in range(len(cur)):
if curvemath.my_dist(cur[i], (x, y)) < m:
m = curvemath.my_dist(cur[i], (x, y))
ind = i
if ind == 0:
p1 = cur[ind]
p2 = cur[ind + 1]
else:
p2 = cur[ind]
p1 = cur[ind - 1]
if not linemath.is_it_left(p1, p2, (x, y)):
m = -m
temp = pd.pd_out(m)
w = min(max(temp, -angular_velocity_limit), angular_velocity_limit)
velocity.angular.z = min(max(w, -angular_velocity_limit),
angular_velocity_limit)
velocity.linear.x = 0.2
elif state == 2:
ready_to_write = False
velocity.linear.x = 0
velocity.angular.z = 0
velocity_pub.publish(velocity)
fhandle.close()
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info",
'r') as fhandle2:
z = fhandle2.read().split('\n')[:-1]
x1 = [float(i.split(' ')[0].split(';')[0]) for i in z]
y1 = [float(i.split(' ')[0].split(';')[1]) for i in z]
plt.plot([i[0] for i in obs], [i[1] for i in obs], 'o')
plt.plot([i[0] for i in obs_s], [i[1] for i in obs_s], 'o')
plt.plot([i[0] for i in cur], [i[1] for i in cur], 'o')
plt.plot(x1, y1, '--', linewidth=2)
plt.title("kp: {}, kd: {}".format(pd.kp, pd.kd))
plt.xlim(-0.75, 0.75)
plt.ylim(-0.75, 0.75)
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'r') as fhandle2:
fignum = int(fhandle2.read().strip())
print fignum
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'w') as fhandle2:
fhandle2.write("{}\n".format(fignum + 1))
plt.savefig(
"/home/ubuntu/catkin_ws/src/alpha_star/scripts/astartest{}.png".
format(fignum))
state = 3
velocity_pub.publish(velocity)
def send_velocity():
global pd0, pd, state, substate, state2start, startangle, ready_to_write
global imuYaw, atan_coefficient
global state, esc_pd
global count_state_0
global X
x, y, theta = X
velocity_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
velocity = Twist()
if time.time() - globalstarttime > 40 and ready_to_write:
state = 2
ready_to_write = False
if state == 0:
velocity.angular.z = min(
max(pd_turn.pd_out(imuYaw), -angular_velocity_limit),
angular_velocity_limit)
if abs(velocity.angular.z) < 0.5:
velocity.angular.z = 0
velocity.linear.x = 0
if abs(linemath.norm(imuYaw - startangle)) < 0.17:
count_state_0 += 1
if count_state_0 >= 5:
velocity.angular.z = 0
state = 1
elif state == 1:
try:
fhandle.write("{};{} {}".format(x, y, ((x - end[0])**2 +
(y - end[1])**2)**(1 / 2.)))
except:
pass
begin_time = time.time()
if ((x - end[0])**2 + (y - end[1])**2)**(1 / 2.) < 0.05:
state = 2
m = 1000
ind = 0
for i in range(len(cur)):
if curvemath.my_dist(cur[i], (x, y)) < m:
m = curvemath.my_dist(cur[i], (x, y))
ind = i
temp = int(0.04 * resolution / size)
print temp
if ind + temp < len(cur):
p1 = cur[ind + temp - 1]
p2 = cur[ind + temp]
elif ind + temp >= len(cur):
esc_pd = True
elif ind == 0:
p1 = cur[ind]
p2 = cur[ind + 1]
else:
p2 = cur[ind]
p1 = cur[ind - 1]
try:
fhandle.write(" beforem: {},".format(m))
except:
pass
if not esc_pd:
curve_angle = np.arctan2(p2[1] - p1[1], p2[0] - p1[0])
m = (atan_coefficient * m)**2
if linemath.is_it_left(p1, p2, (x, y)):
m = -m
angle_setpoint = linemath.norm(np.arctan(m) + curve_angle)
else:
curve_angle = 0
angle_setpoint = linemath.norm(
np.arctan2(cur[-1][1] - y, cur[-1][0] - x))
angle_setpoint += 2 * np.pi
print "angle: {}".format(angle_setpoint)
print "rangle: {}".format(
linemath.norm(imuYaw + startang_robot) + 2 * np.pi)
pd_turn.setpoint = angle_setpoint
temp = pd_turn.pd_out(
linemath.norm(imuYaw + startang_robot) + 2 * np.pi)
w = min(max(temp, -angular_velocity_limit), angular_velocity_limit)
velocity.angular.z = w
velocity.linear.x = 0.2
try:
fhandle.write(" curve:{}, m: {}, setpoint: {}\n".format(
curve_angle, m, angle_setpoint))
except:
pass
elif state == 2:
ready_to_write = False
velocity.linear.x = 0
velocity.angular.z = 0
velocity_pub.publish(velocity)
fhandle.close()
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/info",
'r') as fhandle2:
z = fhandle2.read().split('\n')[:-1]
x1 = [float(i.split(' ')[0].split(';')[0]) for i in z]
y1 = [float(i.split(' ')[0].split(';')[1]) for i in z]
plt.plot([i[0] for i in obs], [i[1] for i in obs], 'o')
plt.plot([i[0] for i in obs_s], [i[1] for i in obs_s], 'o')
plt.plot([i[0] for i in cur], [i[1] for i in cur], 'o')
plt.plot(x1, y1, '--', linewidth=2)
plt.title("C: {}".format(atan_coefficient))
plt.xlim(-0.75, 0.75)
plt.ylim(-0.75, 0.75)
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'r') as fhandle2:
fignum = int(fhandle2.read().strip())
print fignum
with open("/home/ubuntu/catkin_ws/src/alpha_star/scripts/fignum",
'w') as fhandle2:
fhandle2.write("{}\n".format(fignum + 1))
plt.savefig(
"/home/ubuntu/catkin_ws/src/alpha_star/scripts/astartest{}.png".
format(fignum))
state = 3
velocity_pub.publish(velocity)