def run():
    rospy.init_node('boundary_tracker')

    print '--Scarab tracker starting--'


    ## Listen for the localization
    rospy.Subscriber('/vicon/Scarab_DCT_S45', Subject, localization_callback, queue_size=1)
    # Topic to control the robot velocity
    velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)


    # initial time
    init_time = time.time()

    # Load boundaries
    boundaries = pickle.load(open("boundary.p", "rb"))

    log = []



    # Desired angular speed
    Omeg = .3
    # attarction
    Attrac = .6

    drawer = RobotDrawer()
    vel = Twist()


    # ####### Control Loop ###########
    while not rospy.is_shutdown():
        rospy.sleep(0.1)

        # Compute boundary time
        t = time.time()
        boundary_time = int(t - init_time) + 30

        if boundary_time == len(boundaries):
            pickle.dump(log, open("experiment%d.p" % t, "wb"))

            # Stop the robot
            vel.linear.x, vel.angular.z = 0., 0
            velPub.publish(vel)
            break

        boundary = np.array(boundaries[boundary_time])
        boundary[:, 0] -= 1
        boundary[:, 1] -= .7
        boundary[:] *= 1.5

        drawer.draw_polygons([boundary])

        if pose is None:
            print 'Error: no localization.1'
            continue


        ##### Robot pose
        rx, ry = pose.position.x, pose.position.y
        quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
        rth = euler_from_quaternion(quat)[2]

        drawer.update_robots([(rx, ry, rth)])

        #### Control
        clst = point_to_follow(Polygon(boundary), Point((rx, ry)), .2)

        # vector to approach to the desired point
        vr = np.array([clst[0] - rx, clst[1] - ry])

        # Total vector
        vtotal = 2 * vr


        # Feedback linearization
        vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
        vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1

        ## LOG
        log.append((boundary_time, t, rx, ry, rth))

        # vel.linear.x, vel.angular.z = 0., 0
        # print "v=%f w=%f" % (vel.linear.x, vel.angular.z)
        print boundary_time

        velPub.publish(vel)
Пример #2
0
def run():
    rospy.init_node('boundary_tracker')

    print '--Scarab tracker starting--'

    ## Listen for the localization
    rospy.Subscriber('/vicon/Scarab_DCT_S45',
                     Subject,
                     localization_callback,
                     queue_size=1)
    # Topic to control the robot velocity
    velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

    # Desired angular speed
    Omeg = .5
    # attarction
    Attrac = 1.5

    drawer = RobotDrawer()

    # ####### Control Loop ###########
    while not rospy.is_shutdown():
        rospy.sleep(0.1)

        if pose is None:
            print 'Error: no localization.1'
            continue

        theta = np.linspace(0, 2 * pi, 50)
        boundary = [(cos(th1), sin(th1)) for th1 in theta]

        drawer.draw_polygons([boundary])

        # Robot pose
        rx, ry = pose.position.x, pose.position.y
        quat = [
            pose.orientation.x, pose.orientation.y, pose.orientation.z,
            pose.orientation.w
        ]
        rth = euler_from_quaternion(quat)[2]

        drawer.update_robots([(rx, ry, rth)])

        # Circle
        map_theta = atan2(ry, rx)

        # Tangent
        vt = np.array([-sin(map_theta), cos(map_theta)])
        # Atractor to the unitary radious
        vr = np.array([cos(map_theta), sin(map_theta)]) - np.array([rx, ry])

        # Total vector
        vtotal = Attrac * vr + Omeg * vt

        vel = Twist()
        # Feedback linearization
        vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
        vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1

        # The angle is in the contrary direction
        # vel.angular.z = .2 * (atan2(vtotal[1], vtotal[0]) - rth)
        # vel.angular.z =  0.2 * (map_theta - rth + pi)
        # vel.angular.z =  0.2 * (0- rth)
        # print degrees(rth), degrees(atan2(ry, rx)), vel.angular.z

        # vel.linear.x, vel.angular.z = 0., 0
        print "v=%f w=%f" % (vel.linear.x, vel.angular.z)

        velPub.publish(vel)
Пример #3
0
def run():
    rospy.init_node('boundary_tracker')

    print '--Scarab tracker starting--'


    ## Listen for the localization
    rospy.Subscriber('/vicon/Scarab_DCT_S45', Subject, localization_callback, queue_size=1)
    # Topic to control the robot velocity
    velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)


    # Desired angular speed
    Omeg = .5
    # attarction
    Attrac = 1.5


    drawer = RobotDrawer()

    # ####### Control Loop ###########
    while not rospy.is_shutdown():
        rospy.sleep(0.1)

        if pose is None:
            print 'Error: no localization.1'
            continue

        theta = np.linspace(0, 2 * pi, 50)
        boundary = [(cos(th1), sin(th1)) for th1 in theta]

        drawer.draw_polygons([boundary])

        # Robot pose
        rx, ry = pose.position.x, pose.position.y
        quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
        rth = euler_from_quaternion(quat)[2]

        drawer.update_robots([(rx, ry, rth)])

        # Circle        
        map_theta = atan2(ry, rx)

        # Tangent
        vt = np.array([-sin(map_theta), cos(map_theta)])
        # Atractor to the unitary radious
        vr = np.array([cos(map_theta), sin(map_theta)]) - np.array([rx, ry])

        # Total vector
        vtotal = Attrac * vr + Omeg * vt

        vel = Twist()
        # Feedback linearization
        vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
        vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1

        # The angle is in the contrary direction
        # vel.angular.z = .2 * (atan2(vtotal[1], vtotal[0]) - rth)
        # vel.angular.z =  0.2 * (map_theta - rth + pi)
        # vel.angular.z =  0.2 * (0- rth)
        # print degrees(rth), degrees(atan2(ry, rx)), vel.angular.z

        # vel.linear.x, vel.angular.z = 0., 0
        print "v=%f w=%f" % (vel.linear.x, vel.angular.z)

        velPub.publish(vel)
Пример #4
0
def run():
    rospy.init_node('boundary_tracker')

    print '--Scarab tracker starting--'


    ## Listen for the localization
    rospy.Subscriber('/vicon/Scarab_DCT_S45', Subject, localization_callback, queue_size=1)
    # Topic to control the robot velocity
    velPub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)


    # initial time
    init_time = time.time()

    # Load boundaries
    boundaries = pickle.load(open("boundary.p", "rb"))

    log = []



    # Desired angular speed
    Omeg = .3
    # attarction
    Attrac = .6

    drawer = RobotDrawer()
    vel = Twist()


    # ####### Control Loop ###########
    while not rospy.is_shutdown():
        rospy.sleep(0.1)

        # Compute boundary time
        t = time.time()
        boundary_time = int(t - init_time) + 30

        if boundary_time == len(boundaries):
            pickle.dump(log, open("experiment%d.p" % t, "wb"))

            # Stop the robot
            vel.linear.x, vel.angular.z = 0., 0
            velPub.publish(vel)
            break

        boundary = np.array(boundaries[boundary_time])
        boundary[:, 0] -= 1
        boundary[:, 1] -= .7
        boundary[:] *= 1.5

        drawer.draw_polygons([boundary])

        if pose is None:
            print 'Error: no localization.1'
            continue


        ##### Robot pose
        rx, ry = pose.position.x, pose.position.y
        quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
        rth = euler_from_quaternion(quat)[2]

        drawer.update_robots([(rx, ry, rth)])

        #### Control
        # Tangent
        # vt = np.array([-sin(map_theta), cos(map_theta)])
        clst = closest_on_polygon(Polygon(boundary), Point((rx, ry)))
        vt = tanget_dir(clst, boundary)

        # Attractor to the unitary radius
        # vector to approach
        vr = np.array([clst[0] - rx, clst[1] - ry])

        # Total vector
        vtotal = Attrac * vr + Omeg * vt


        # Feedback linearization
        vel.linear.x = vtotal[0] * cos(rth) + vtotal[1] * sin(rth)
        vel.angular.z = -vtotal[0] * sin(rth) / .1 + vtotal[1] * cos(rth) / .1

        ## LOG
        log.append((boundary_time, t, rx, ry, rth))

        # vel.linear.x, vel.angular.z = 0., 0
        # print "v=%f w=%f" % (vel.linear.x, vel.angular.z)
        print boundary_time

        velPub.publish(vel)