Example #1
0
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)
Example #2
0
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
Example #3
0
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)
Example #4
0
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)
Example #5
0
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)