def comm(ip, port, bind_ip, signal_q, control_q, outgoing_q,
loop=None, context=None):
""" Communications coroutine
Input Channels:
ZMQ router: from outside world
signal_q: to break waits on the router
outgoing_q: data that needs to be sent out on the router
Output Channels:
ZMQ router: to the outside world
control_q: put messages from outside world here for handling
Interacts with:
send, control
"""
loop = loop or asyncio.get_event_loop()
context = context or zmq.Context()
router = context.socket(zmq.ROUTER)
router.bind('tcp://%s:%d' % (bind_ip, port))
dealer = context.socket(zmq.DEALER)
dealer.connect('tcp://127.0.0.1:%d' % port)
wait_signal = Task(signal_q.get(), loop=loop)
while True:
wait_router = delay(loop, router.recv_multipart)
[first], [other] = yield From(asyncio.wait([wait_router, wait_signal],
return_when=asyncio.FIRST_COMPLETED))
if first is wait_signal: # Interrupt socket recv
dealer.send(b'break')
addr, data = yield From(wait_router) # should be fast
assert data == b'break'
while not outgoing_q.empty(): # Flow data out
addr, msg = outgoing_q.get_nowait()
router.send_multipart([addr, msg])
print("Message sent")
if first is wait_signal: # Handle internal messages
msg = wait_signal.result()
if msg == b'close':
control_q.put_nowait((None, b'close'))
break
elif msg == b'interrupt':
wait_signal = Task(signal_q.get(), loop=loop)
continue
elif first is wait_router: # Handle external messages
addr, byts = wait_router.result()
msg = loads(byts)
print("Communication received: %s" % str(msg))
control_q.put_nowait((addr, msg))
router.close(linger=2)
dealer.close(linger=2)
raise Return("Comm done")
def start(options):
gait_driver = MechDriver(options)
driver_task = Task(gait_driver.run())
input_task = Task(handle_input(gait_driver))
done, pending = yield From(
asyncio.wait([driver_task, input_task],
return_when=asyncio.FIRST_EXCEPTION))
for x in done:
x.result()
def handle_mouse_move(self, cursor):
point_mm = self.coord_to_point((cursor.x(), cursor.y()))
self.servo_tab.ui.resultsEdit.setText('')
result = self.leg_ik.do_ik(point_mm)
if not result.valid():
# This option isn't possible
return
message = ''
command = {}
for joint in result.joints:
command[joint.ident] = joint.angle_deg
message += '%d: %f\n' % (joint.ident, joint.angle_deg)
if hasattr(self.leg_ik, 'forward'):
forward = self.leg_ik.forward(result)
message += '\nshoulder: %.1f,%.1f,%.1f\n' % (
forward.shoulder.x, forward.shoulder.y, forward.shoulder.z)
message += 'femur: %.1f,%.1f,%.1f\n' % (
forward.femur.x, forward.femur.y, forward.femur.z)
message += 'tibia: %.1f,%.1f,%.1f\n' % (
forward.tibia.x, forward.tibia.y, forward.tibia.z)
self.servo_tab.ui.resultsEdit.setText(message)
Task(self.servo_tab.set_pose(command))
def handle_servo_spin(self, servo_id, event):
if self.servo_update.value:
return
with self.servo_update:
control = self.servo_controls[servo_id]
value = control['doublespin'].value()
control['slider'].setSliderPosition(int(value))
Task(self.set_single_pose(servo_id, value))
def update_connected(self, value):
self.ui.controlGroup.setEnabled(value)
self.ui.posesGroup.setEnabled(value)
if self.monitor_thread is not None:
self.monitor_thread.cancel()
self.monitor_thread = None
if value:
self.handle_power()
self.monitor_thread = Task(self.monitor_status())
def main():
logging.basicConfig(level=logging.WARN, stream=sys.stdout)
parser = optparse.OptionParser(description=__doc__)
MechDriver.add_options(parser)
options, args = parser.parse_args()
task = Task(start(options))
asyncio.get_event_loop().run_until_complete(task)
def run(self):
kwargs = {}
if self.options.serial_port:
kwargs['serial_port'] = self.options.serial_port
if self.options.model_name:
kwargs['model_name'] = self.options.model_name
self.servo = yield From(selector.select_servo(
self.options.servo,
**kwargs))
self.driver = GaitDriver(self.gait, self.servo)
idle_task = Task(self._make_idle())
driver_task = Task(self.driver.run())
done, pending = yield From(
asyncio.wait([idle_task, driver_task],
return_when=asyncio.FIRST_EXCEPTION))
for x in done:
x.result()
def handle_mouse_move(self, cursor):
if self.servo_tab.controller is None:
return
point_mm = self.coord_to_point((cursor.x(), cursor.y()))
result = self.leg_ik.do_ik(point_mm)
if result is None:
# This option isn't possible
return
Task(self.servo_tab.controller.set_pose(result.command_dict()))
def handle_current_changed(self, index=2):
if index != 2:
# Make sure we're not still playing.
self.ui.playbackSingleButton.setChecked(False)
self.ui.playbackRepeatButton.setChecked(False)
self.ui.playbackSlowRepeatButton.setChecked(False)
if self.servo_tab.controller:
Task(
self.servo_tab.controller.enable_power(
selector.POWER_BRAKE))
return
if self.servo_tab.controller:
Task(self.servo_tab.controller.enable_power(selector.POWER_ENABLE))
# Update the leg list widget.
available_legs = self.ikconfig_tab.get_all_legs()
enabled_legs = set(self.ikconfig_tab.get_enabled_legs())
for leg_num in available_legs:
leg_str = str(leg_num)
if not self.ui.gaitLegList.findItems(leg_str,
QtCore.Qt.MatchExactly):
self.ui.gaitLegList.addItem(leg_str)
items = self.ui.gaitLegList.findItems(leg_str,
QtCore.Qt.MatchExactly)
item = items[0]
if leg_num in enabled_legs:
item.setFlags(QtCore.Qt.ItemIsEnabled
| QtCore.Qt.ItemIsSelectable)
else:
item.setFlags(0)
self.handle_leg_change(self.ui.gaitLegList.currentItem())
# Make sure that our configuration is fully up to date.
self.handle_gait_config_change()
self.command_widget.fit_in_view()
def update_allowable(self, config, command):
self.next_config = config.copy()
self.next_command = command.copy()
for (x, y), rect in self.usable_rects.iteritems():
old_brush = rect.brush()
old_color = old_brush.color()
rect.setBrush(
QtGui.QBrush(
QtGui.QColor(old_color.red(), old_color.green(),
old_color.blue(), 64)))
Task(self._really_update_allowable())
def render_state(self, state):
# Render the phase line in the gait graph.
self.gait_graph_display.set_phase(state.phase % 1.0)
self.gait_geometry_display.set_state(state)
if self.servo_tab.controller:
try:
command = state.command_dict()
except ripple.NotSupported:
return
self.next_command = command
if (self.current_command is not None
and not self.current_command.done()):
return
self.current_command = Task(self.set_next_pose())
def render_state(self, state):
# Render the phase line in the gait graph.
self.gait_graph_display.set_phase(state.phase % 1.0)
self.gait_geometry_display.set_state(state)
if self.servo_tab.controller:
joint_command = self.ripple.make_joint_command(state)
command = {}
for joint in joint_command.joints:
command[joint.servo_number] = joint.angle_deg
self.next_command = command
if (self.current_command is not None
and not self.current_command.done()):
return
self.current_command = Task(self.set_next_pose())
def start():
Task(callback())
def handle_mouse_release(self):
if self.servo_tab.controller is None:
return
Task(self.servo_tab.controller.enable_power(selector.POWER_BRAKE))
def handle_mouse_release(self):
Task(self.servo_tab.set_power('brake'))
def handle_mouse_press(self, cursor):
Task(self.servo_tab.set_power('drive'))
self.handle_mouse_move(cursor)
def create_task(self, coro):
return Task(coro)
def handle_mouse_press(self, cursor):
if self.servo_tab.controller is None:
return
Task(self.servo_tab.controller.enable_power(selector.POWER_ENABLE))
self.handle_mouse_move(cursor)
