lcm_robot_driver

In [1]:

    

# Cross-notebook include shim
with open("nbinclude.ipynb") as nbinclude_f: # don't rename nbinclude_f
    import IPython.nbformat.current
    get_ipython().run_cell(IPython.nbformat.current.read(nbinclude_f, 'json').worksheets[0].cells[0].input)


    

In [2]:

    

import time, lcm
from python.forseti2 import piemos_cmd
from python.fearing import xbox_joystick_state


    

In [3]:

    

class RobotRunner:
    def __init__(self, robot):
        self.robot=robot

    def handle_joy(self, chan, data):
        msg = xbox_joystick_state.decode(data)
        l,r = msg.axes[1], msg.axes[4]
        self.robot.cmd(l,r)
        
    def handle_piemos(self, channel, data):
        msg=piemos_cmd.decode(data)
        if msg.enabled:
            self.robot.enable()
        else:
            self.robot.disable()

    def lcm_run(self):
        lc=lcm.LCM('udpm://239.255.76.67:7667?ttl=1')
        sub=lc.subscribe("robot0/cmd", self.handle_joy)
        sub2=lc.subscribe("piemos0/cmd", self.handle_piemos)
        self.running = True
        while self.running:
            lc.handle()
#             for tick in self.ticks:
#                 tick()
            time.sleep(.01)


    

In [4]:

    

nbinclude("robot")


    

In [5]:

    

r=Robot()


    

In [6]:

    

r.cmd(1,1)


    

In [7]:

    

r.enable()


    

In [8]:

    

# r.cmd(0,0)


    

In [ ]:

    

rr=RobotRunner(r)


    

In [ ]:

    

rr.lcm_run()


    

In [ ]:

    




    

In [ ]: