@@ -0,0 +1,2498 @@
+import lcm%0Aimport drc as lcmdrc%0Aimport datetime%0Aimport select%0A%0Aclass LCMPoller(object):%0A%0A    def __init__(self, lc):%0A        self.lc = lc%0A        self.poll = select.poll()%0A        self.poll.register(self.lc.fileno())%0A%0A    def handleLCM(self, timeout=100):%0A        if self.poll.poll(timeout):%0A            self.lc.handle()%0A%0A%0Adef getTimeString():%0A    return datetime.datetime.now().strftime('%25H:%25M:%25S')%0A%0A%0AplanMsgCount = 0%0A%0A%0Adef onCommittedRobotPlan(channel, msgBytes):%0A%0A    global planMsgCount%0A    msg = lcmdrc.robot_plan_t.decode(msgBytes)%0A    print 'received message %25d on %25s with utime %25r at: %25s' %25 (planMsgCount, channel, msg.utime, getTimeString())%0A    planMsgCount += 1%0A%0A%0AconfMsgCount = 0%0A%0Adef onConfigurationTraj(channel, msgBytes):%0A    global confMsgCount%0A    print 'received message %25d on %25s at: %25s' %25 (confMsgCount, channel, getTimeString())%0A    confMsgCount += 1%0A%0Adef main():%0A%0A    lc = lcm.LCM()%0A    poller = LCMPoller(lc)%0A%0A%0A    lc.subscribe('COMMITTED_ROBOT_PLAN', onCommittedRobotPlan)%0A    lc.subscribe('CONFIGURATION_TRAJ', onConfigurationTraj)%0A%0A    while True:%0A        poller.handleLCM()%0A%0A%0Aif __name__ == '__main__':%0A    main()%0A%0A%0Aclass PlanPressureCommander(AtlasPressureCommander):%0A    def __init__(self,%0A                 plan_map=DEFAULT_PLAN_PRESSURE_MAP,%0A                 behavior_map=DEFAULT_BEHAVIOR_PRESSURE_MAP,%0A                 **kwargs):%0A        super(PlanPressureCommander, self).__init__(**kwargs)%0A        self.plan_map = plan_map%0A        self.behavior_map = behavior_map%0A        self._setup_subscriptions()%0A%0A    def _setup_subscriptions(self):%0A        self.lc.subscribe('CONTROLLER_STATUS', self.handle_controller_status)%0A        self.lc.subscribe('ATLAS_BEHAVIOR_COMMAND', self.handle_behavior)%0A%0A    def handle_controller_status(self, channel, msg):%0A        if isinstance(msg, str):%0A            msg = lcmdrc.controller_status_t.decode(msg)%0A%0A        if msg.state in self.plan_map:%0A            self.desired_psi = self.plan_map%5Bmsg.state%5D%0A            self.publish_pump_command()%0A%0A    def handle_behavior(self, channel, msg):%0A        if isinstance(msg, str):%0A            msg = lcmdrc.atlas_behavior_command_t.decode(msg)%0A%0A        s = msg.command.lower()%0A        if s in self.behavior_map:%0A            self.desired_psi = self.behavior_map%5Bs%5D%0A            self.publish_pump_command()%0A%0A    def run(self):%0A        while True:%0A            time.sleep(0.1)%0A            self.lc.handle()%0A%0Adef main():%0A    mon = PlanPressureCommander()%0A    print %22Pressure Command: ready%22%0A    mon.run()%0A%0A

add script to help debug dropped plan messages