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)