示例#1
0
def func(input, output):
    print 'Odometry process running...'

    odometry_queue = Queue()
    laser_queue = Queue()
    viewer = http_viewer.HttpViewer(port_web_server, laser_queue,
                                    odometry_queue)

    S = float(input.get())
    #print 'S=' + str(S)

    anterior = [0, 0, 0]

    output.put('m')
    msg = input.get().split("\n")
    L_ini = float(msg[4].split(',')[1])
    R_ini = float(msg[8].split(',')[1])
    anterior = [L_ini, R_ini, 0, 0, 0]

    motReq = False

    quite = False

    while not quite:

        try:
            msg = input.get_nowait()
            if msg == "q":
                print "NO ME CIERRO JODER"
                printVerbose("quite", verboseCommands)
                quite = True
                viewer.quit()

            elif msg == "o2":
                output.put(
                    str(anterior[2]) + "," + str(anterior[3]) + "," +
                    str(anterior[4]))

            elif msg[0] == "m":  #Obtencion de datos del motor
                printVerbose("motors", verboseCommands)
                print "ENTRO A ODOMETRY MODULE M"
                msg = msg.split(
                    "\n")  #dentro de msg se encuentran los valores del motor
                L = float(msg[4].split(',')[1])
                R = float(msg[8].split(',')[1])
                anterior = odometry(anterior, L, R)
                odometry_queue.put([(anterior[2], anterior[3]), (100, 100)])
                print "\rOdometry: x:" + str(anterior[2]) + " y:" + str(
                    anterior[3]) + " theta:" + str(anterior[4] * 180 / math.pi)
                motReq = False
            elif msg[0] == "l":  #Obtencion de datos del laser
                printVerbose("laser", verboseCommands)
                msg = msg[
                    1:]  #dentro de msg se encuentran los valores del laser
            elif not motReq:
                #odometry(anterior,L,R)
                output.put("motors")
                motReq = True
                time.sleep(0.1)

        except Empty:
            if not motReq:
                #odometry(anterior,L,R)
                output.put("motors")
                motReq = True
                time.sleep(0.1)
示例#2
0
    sensor = []
    for i in range(0, 5):
        sensor.append(dist_max)
        for j in range(-60 + i * 24, -60 + 24 + i * 24):
            if int(laser[j][3]) == 0:
                if (int(laser[j][1]) < sensor[i]):
                    sensor[i] = int(laser[j][1])

    return sensor


if __name__ == "__main__":

    odometry_queue = Queue()
    laser_queue = Queue()
    viewer = http_viewer.HttpViewer(port_web_server, laser_queue,
                                    odometry_queue)
    print "To open the viewer go to: http:\\\\192.168.100.1:" + str(
        port_web_server)
    print "To see the log run in a shell the next comnnad: 'tail -f log.txt'"
    print "Press 'Q' to stop the execution."

    # Iniciamos -------------------------------
    global ser
    ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05)
    print 'INI: SerialPort -> on'
    envia(ser, 'TestMode On', 0.2)
    envia(ser, 'PlaySound 1')
    print 'INI: TestMode -> on'
    envia(ser, 'SetMotor RWheelEnable LWheelEnable')
    print 'INI: SetMotor -> on'
    envia(ser, 'SetLDSRotation On', 0.2, False)
示例#3
0
def func(input, output):
    print 'Odometry process running...'
    laser_set = Set()
    odometry_queue = Queue()
    laser_queue = Queue()
    viewer = http_viewer.HttpViewer(port_web_server, laser_queue,
                                    odometry_queue)

    S = float(input.get())
    #print 'S=' + str(S)

    anterior = [0, 0, 0]

    output.put('m')
    msg = input.get().split("\n")
    L_ini = float(msg[4].split(',')[1])
    R_ini = float(msg[8].split(',')[1])
    anterior = [L_ini, R_ini, 0, 0, 0]

    laserMotor = True
    motReq = False
    lasReq = False
    numOdometrys = 0
    quite = False

    while not quite:

        try:
            msg = input.get_nowait()
            if msg == "q":
                printVerbose("quite", verboseCommands)
                quite = True
                viewer.quit()

            elif msg == "o2":
                output.put(
                    str(anterior[2]) + "," + str(anterior[3]) + "," +
                    str(anterior[4]))

            elif msg[0] == "m":  #Obtencion de datos del motor
                printVerbose("motors", verboseCommands)
                msg = msg.split(
                    "\n")  #dentro de msg se encuentran los valores del motor
                L = float(msg[4].split(',')[1])
                R = float(msg[8].split(',')[1])
                anterior = odometry(anterior, L, R)
                odometry_queue.put([(anterior[2], anterior[3]), (100, 100)])
                print "\rOdometry: x:" + str(anterior[2]) + " y:" + str(
                    anterior[3]) + " theta:" + str(anterior[4] * 180 / math.pi)
                motReq = False
                if (numOdometrys > 5):
                    laserMotor = False
                numOdometrys += 1

            elif msg[0] == "l":  #Obtencion de datos del laser
                printVerbose("laser", verboseCommands)
                msg = msg[
                    1:]  #dentro de msg se encuentran los valores del laser

                laser_points = get_laser(msg)

                var = polar_to_cart(laser_points)

                if len(var) > 0:
                    var = cart_to_world(var, anterior)

                    for i in var:
                        tuple = (int(i[0]), int(i[1]))
                        if tuple not in laser_set:
                            laser_queue.put([(i[0], i[1]), (100, 100)])
                            laser_set.add(tuple)
                lasReq = False
                laserMotor = True
                numOdometrys = 0

            elif not motReq:
                #odometry(anterior,L,R)
                output.put("motors")
                motReq = True
                time.sleep(0.1)

        except Empty:
            if not motReq and not lasReq:
                if laserMotor:
                    #odometry(anterior,L,R)
                    print "\nEnviem Calc Odometry"
                    output.put("motors")
                    motReq = True
                    time.sleep(0.1)
                else:
                    print "\nEnviem Calc Laser"
                    output.put("laser")
                    lasReq = True
                    time.sleep(0.1)
            Pk1[1][1]) + ' Pk_thth: ' + str(Pk1[2][2])


# Funcion ejecutada por el thread auxiliar encargado de gestionar la entrada de datos no bloqueante
def f(q):
    r = ''
    while r != "q" and r != "b":
        r = getch()
        q.put(r)


if __name__ == "__main__":
    # Initialize WebServer
    laser_queue = Queue()
    pose_queue = Queue()
    viewer = http_viewer.HttpViewer(port_web_server, laser_queue, pose_queue)
    print "To open the viewer go to: http:\\\\192.168.100.1:" + str(
        port_web_server)

    # Initialize Robot
    global ser
    ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05)
    envia(ser, 'TestMode On')
    envia(ser, 'PlaySound 1')
    # Habilitamos motores y laser
    envia(ser, 'SetMotor RWheelEnable LWheelEnable')
    envia(ser, 'SetLDSRotation On')

    #Initialize Robot parameters
    speed = 0  # en mm/s
    tita_dot = 0
示例#5
0
		envia(ser,'TestMode On', 0.2)

		envia(ser,'PlaySound 1', 0.3)

		envia(ser,'SetMotor RWheelEnable LWheelEnable', 0.2)

		envia(ser,'SetLDSRotation On',0.1)

		old_settings = termios.tcgetattr(sys.stdin)
		tty.setcbreak(sys.stdin.fileno())

		port_web_server=123
		r_queue = Queue()
		l_queue = Queue()
		viewer = http_viewer.HttpViewer(port_web_server, l_queue, r_queue)
		#viewer.main_http_server()

		# Parametros Robot.
		S = 121.5		# en mm
		distancia_L = 0	# en mm
		distancia_R = 0	# en mm
		speed = 0 		# en mm/s
		tita_dot = 0
		tiempo = 20		
		direccion = 0

		L_ini, R_ini = get_motors()
		x_world=x_ini
		y_world=y_ini
		theta_world=theta_ini
示例#6
0
 def viewer_start(self, port_web_server):
     self.laser_queue = Queue()
     self.odometry_queue = Queue()
     self.viewer = http_viewer.HttpViewer(port_web_server, self.laser_queue,
                                          self.odometry_queue)
     debug("Started Http Viewer at port %i" % port_web_server)