def main():
    global P
    global CMD_VEL
    # Uncomment for debugging if nesssary, recomment before turning in.
    # rp.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
    rp.Subscriber('/robot/base_scan', LaserScan, got_scan)  # movement/sensor particle update
    rp.Subscriber('/robot/cmd_vel', Twist, got_vel)
    #actual robot pose only used to show it on map not for partcile filter
    rp.Subscriber('/stage/base_pose_ground_truth', Odometry, actual_pose)  #actual robot pose

    #removed due to wander node
    #CMD_VEL = rp.Publisher('/robot/cmd_vel', Twist, queue_size=1)

    # create initial particles
    for p in xrange(PARTICLE_COUNT):
        r = ROBOT()
        r.set_pose(5, 5.6, 0)  # map space
        P.append(r)

    mcl_tools.mcl_init('particle_2d')
    mcl_tools.mcl_run_viz()
Example #2
0

def particle_filter(ps, control, scan):
    # FIXME: Should really particle filter.
    return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]


def got_scan(msg):
    global parset
    global cmd_vel

    control = (0.0, 0.0)

    parset = particle_filter(parset, control, msg)
    mcl_tools.show_particles(parset)

    cmd = Twist()
    (cmd.linear.x, cmd.angular.z) = control
    cmd_vel.publish(cmd)


if __name__ == '__main__':
    # Uncomment for debugging if nesssary, recomment before turning in.
    #rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)

    rospy.Subscriber('/robot/base_scan', LaserScan, got_scan)
    cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist, queue_size=1)

    mcl_tools.mcl_init('with_weights')
    mcl_tools.mcl_run_viz()
Example #3
0
            centroid = motion_update(centroid)
            rospy.sleep(0.1)


if __name__ == '__main__':
    # Uncomment for debugging if nesssary, recomment before turning in.
    #rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
    try:
        import psyco
        psyco.full()
    except IOError:
        pass

    cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist)
    rospy.Subscriber('/robot/base_scan',
                     LaserScan,
                     callback=got_scan,
                     queue_size=10,
                     tcp_nodelay=True)
    rospy.Subscriber('/robot/base_scan',
                     LaserScan,
                     callback=controller,
                     queue_size=10,
                     tcp_nodelay=True)

    clustering_thread = threading.Thread(target=clustering)
    clustering_thread.start()

    mcl_tools.mcl_init('sample_hw7')
    mcl_tools.mcl_run_viz()
Example #4
0
def got_scan(msg):
    global parset, odometry

    if odometry is not None:
        # print odometry
        linear_speed = odometry.twist.twist.linear.x
        angular_speed = odometry.twist.twist.angular.z
        control = (linear_speed, angular_speed)
        # print control

        parset = particle_filter(parset, control, msg)  # run particle filter
        mcl_tools.show_particles(parset)  # render particle cloud
    else:
        print "odometry is none"


def got_odom(msg):
    global odometry
    odometry = msg

def got_map(msg):
    print msg

if __name__ == '__main__':
    rp.Subscriber('/scan', LaserScan, got_scan, queue_size=1)
    rp.Subscriber('/odom', Odometry, got_odom, queue_size=1)
    # rp.Subscriber('/map', )

    mcl_tools.mcl_init('localizer_node')
    mcl_tools.mcl_run_viz()
Example #5
0

def particle_filter(ps, control, scan):
    # FIXME: Should really particle filter.
    return [mcl_tools.random_particle() for ii in range(PAR_COUNT)]


def got_scan(msg):
    global parset
    global cmd_vel

    control = (0.0, 0.0)

    parset = particle_filter(parset, control, msg)
    mcl_tools.show_particles(parset)

    cmd = Twist()
    (cmd.linear.x, cmd.angular.z) = control
    cmd_vel.publish(cmd)


if __name__ == '__main__':
    # Uncomment for debugging if nesssary, recomment before turning in.
    #rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)

    rospy.Subscriber('/robot/base_scan', LaserScan, got_scan)
    cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist, queue_size=1)

    mcl_tools.mcl_init('no_weights')
    mcl_tools.mcl_run_viz()
Example #6
0
        Wait = False

        rospy.sleep(0.1)
        for i in xrange(20):
            centroid = motion_update(centroid)
            rospy.sleep(0.1)



if __name__ == '__main__':
    # Uncomment for debugging if nesssary, recomment before turning in.
    #rospy.Subscriber('/stage/base_pose_ground_truth', Odometry, mcl_debug.got_odom)
    try:
        import psyco
        psyco.full()
    except IOError:
        pass

    cmd_vel = rospy.Publisher('/robot/cmd_vel', Twist)
    rospy.Subscriber('/robot/base_scan', LaserScan, callback=got_scan,
            queue_size = 10, tcp_nodelay = True)
    rospy.Subscriber('/robot/base_scan', LaserScan, callback=controller,
            queue_size = 10, tcp_nodelay = True)

    clustering_thread = threading.Thread(target=clustering)
    clustering_thread.start()

    mcl_tools.mcl_init('sample_hw7')
    mcl_tools.mcl_run_viz()