main.py

__author__ = 'xuepeng'

from SerialMsg import *
from tinyos.message import MoteIF
from time import sleep
from os.path import expanduser
home = expanduser("~")
from DSRC_JobProcessor import *
from create import Create
import sys, os
import signal
from multiprocessing import Process
import multiprocessing

from Queue import Queue

speed = None

class PathJob:
    def __init__(self, action, value):
        self.action = action
        self.value = value


class PathMaintainer:
    def __init__(self):
        # read path
        self.list = []

    def add_job(self, job):
        self.list.append(job)

    def retrieve_one_second_job(self, speed):
        if len(self.list) == 0:
            return None, None, None
        else:
            path_job = self.list[0]
            move_length = speed * 1
            if path_job.action == 'w' or path_job.action == 'b':
                if path_job.value <= move_length:
                    time_to_go = path_job.value/float(speed)
                    self.list.pop(0)
                    return path_job.action, speed, time_to_go
                else:
                    path_job.value -= move_length
                    return path_job.action, speed, 1.0
            else:
                rotation_speed = path_job.value/0.8
                self.list.pop(0)
                return path_job.action, rotation_speed, 0.8

    def read_path(self, filename):
        path_lines = open(filename, 'r')
        for line in path_lines:
            job_line = line.rstrip('\n')
            action, value_str = job_line.split(',')
            value = int(value_str)
            job = PathJob(action, value)
            self.add_job(job)


class SpeedChangeRunner(JobCallback, Thread):
    def __init__(self, robot_port, q):
        Thread.__init__(self)
        # Path
        self.path_maintainer = PathMaintainer()
        filename = home + "/" + "path.txt"
        self.path_maintainer.read_path(filename)

        robot = Create(robot_port)
        # robot = None
        # Job processor
        self.job_processor = JobProcessor(robot)
        self.queue = Queue()
        self.q = q
        self.start()

    def run(self):
        while True:
            print "Start"
            job_speed = self.queue.get()
            print "Job speed"
            action, speed, time_to_go = self.path_maintainer.retrieve_one_second_job(job_speed)
            if action is None:
                print "No job!"
                pass
            else:
                if action == 'w':
                    job = Job(self, 'go', time_to_go, speed, 0)
                    self.job_processor.add_new_job(job)
                elif action == 'b':
                    job = Job(self, 'go', time_to_go, -speed, 0)
                    self.job_processor.add_new_job(job)
                elif action == 'r':
                    job = Job(self, 'go', time_to_go, 0, -speed)
                    self.job_processor.add_new_job(job)
                elif action == 'l':
                    job = Job(self, 'go', time_to_go, 0, speed)
                    self.job_processor.add_new_job(job)

    def signal_receiver(self, signum, stack):
        speed = self.q.get()
        if speed:
            self.queue.put(speed)

    def job_finished(self, action, arg1, arg2, timeExecuted):
        print "Job finished: " + str(action) + ' ' + str(arg1) + ' ' + str(arg2) + ' ' + str(timeExecuted)

    def job_paused(self, action, arg1, arg2, timeExecuted):
        print "Job paused:" + str(action) + ' ' + str(arg1) + ' ' + str(arg2) + ' ' + str(timeExecuted)

    def stop_self(self):
        self.job_processor.stop_processor()

class Mote:
    def __init__(self, pid, q):
        self.mif = MoteIF.MoteIF()
        # Create a MoteIF
        self.mif = MoteIF.MoteIF()
        # Attach a source to it
        self.source = self.mif.addSource("sf@localhost:9002")
        # SomeMessageClass.py would be generated by MIG
        self.mif.addListener(self, SerialMsg)
        self.pid = pid
        self.q = q

    def receive(self, src, msg):
        global speed
        print "Received message: ", msg.get_source(), msg.get_num_msg(), msg.get_total_msg()
        self.q.put(30)
        os.kill(self.pid, signal.SIGUSR1)
        power = 31
        interval = 32
        ts = SerialMsg()
        print "send packet, power_level:send_interval", power, ":", interval
        ts.set_power_level(power)
        ts.set_send_interval(interval)
        self.mif.sendMsg(self.source, 0, ts.get_amType(), 0, ts)


def robot_func(robot_port,q):
    runner = SpeedChangeRunner(robot_port, q)
    signal.signal(signal.SIGUSR1, runner.signal_receiver)
    while True:
        pass

def mote_func(pid,q):
    mote = Mote(pid,q)
    while True:
        pass

if __name__ == '__main__':
    print "Start"
    args = sys.argv
    if len(args) < 2:
        print "need a robot port name!"
        exit()
    robot_port = args[1]
    q = multiprocessing.Queue()
    p1 = Process(target=robot_func, args=(robot_port,q,))
    p1.start()
    time.sleep(0.5)
    p2 = Process(target=mote_func, args=(p1.pid,q,))
    p2.start()

    raw_input("Enter to stop!")
    exit()