def _spnav_event_loop(self): import spnav kill_pending_events = False try: while self._running: if not self.focus_test(): kill_pending_events = True self.cmd._sdof(0., 0., 0., 0., 0., 0.) time.sleep(0.1) continue if kill_pending_events: kill_pending_events = False while spnav.spnav_poll_event() is not None: pass event = spnav.spnav_wait_event() if event.ev_type == spnav.SPNAV_EVENT_MOTION: t, r = event.translation, event.rotation args = (t[0], -t[2], -t[1], r[0], -r[2], -r[1]) args = [v * SCALE for v in args] self.cmd._sdof(*args) elif event.ev_type == spnav.SPNAV_EVENT_BUTTON: if event.press: self.cmd._sdof_button(event.bnum + 1) except spnav.SpnavWaitException: print('SpnavWaitException: spacenavd stopped?') finally: spnav.spnav_close()
def get_spnav_cmd(self): trans_cmd = np.zeros(3, dtype=np.float16) rot_cmd = np.zeros(3, dtype=np.float16) sp_event = spnav.spnav_poll_event() if sp_event is not None and sp_event.ev_type == spnav.SPNAV_EVENT_BUTTON: # print(sp_event.bnum, sp_event.press) if sp_event.bnum == 0 and sp_event.press: self.grasp = not self.grasp print('Gripper: {}'.format('Open' if self.grasp else 'Close')) elif sp_event.bnum == 1 and sp_event.press: self.ctrl_mod = not self.ctrl_mod print('Ctrl Mod: {}'.format( 'Translational' if not self.ctrl_mod else 'Rotational')) elif sp_event is not None and sp_event.ev_type == spnav.SPNAV_EVENT_MOTION: trans_cmd = np.asarray(sp_event.translation, dtype=np.float16) / 1000 rot_cmd = np.asarray(sp_event.rotation, dtype=np.float16) / 500 # print('pos: {}'.format(pos_cmd[0])) eef_vel = np.concatenate((trans_cmd, rot_cmd)) return EEFCommand(self.grasp, self.ctrl_mod, eef_vel)
def test_spnav_poll_event_button(): def button(event): fill_button_event(event) return 1 m = mock_libspnav({'spnav_poll_event': button}) event = spnav.spnav_poll_event() assert event is not None assert_equals(event.bnum, 0) assert_equals(event.press, True)
def test_spnav_poll_event_motion(): def motion(event): fill_motion_event(event) return 1 m = mock_libspnav({'spnav_poll_event': motion}) event = spnav.spnav_poll_event() assert event is not None assert_equals(event.translation, (-1, 2, -3)) assert_equals(event.rotation, (10, -20, 30))
def run(self): """Run the interface.""" # # Remove pending space navigator events from the queue # spnav.spnav_remove_events(spnav.SPNAV_EVENT_ANY) # get space mouse event (without waiting) # event = spnav_wait_event() # blocking wait for the next event # try: event = spnav.spnav_poll_event() # except KeyboardInterrupt: # print '\nQuitting...' # finally: # self.close() # check the event self.updated = True if isinstance(event, spnav.SpnavButtonEvent): # button event if event.bnum == 0: # left self.left_button_pressed = event.press elif event.bnum == 1: # right self.right_button_pressed = event.press elif isinstance(event, spnav.SpnavMotionEvent): # mouse motion event # translation x, y, z = event.translation # x, y, z in spnav self.translation = np.array( [z, -x, y], dtype=np.float) # x (front), y (left), z (up) condition = self.translation < 0 self.translation[condition] /= self.translation_range[0][condition] self.translation[~condition] /= self.translation_range[1][ ~condition] # rotation r, p, y = event.rotation # r, p, y in spnav self.rotation = np.array( [y, -r, p], dtype=np.float) # x (front), y (left), z (up) condition = self.rotation < 0 self.rotation[condition] /= self.rotation_range[0][condition] self.rotation[~condition] /= self.rotation_range[1][~condition] else: self.updated = False # if we should print the events if self.verbose and self.updated: print("Left and Right buttons: {} and {}".format( self.left_button_pressed, self.right_button_pressed)) print("Space mouse translation and orientation: {} and {}".format( self.translation, self.rotation))
def process_spnav_events(self): event = spnav.spnav_poll_event() if event: if event.ev_type == spnav.SPNAV_EVENT_MOTION: self.spnav_states["linear_x"] = event.translation[0] / 350.0 self.spnav_states["linear_y"] = event.translation[2] / 350.0 self.spnav_states["linear_z"] = event.translation[1] / 350.0 self.spnav_states["angular_x"] = event.rotation[2] / 350.0 self.spnav_states["angular_y"] = -(event.rotation[0] / 350.0) self.spnav_states["angular_z"] = -(event.rotation[1] / 350.0) # print "x", self.spnav_states["linear_x"], "\t", "y", self.spnav_states["linear_y"], "\t", "z", self.spnav_states["linear_z"] # print "x", self.spnav_states["angular_x"], "\t", "y", self.spnav_states["angular_y"], "\t", "z", self.spnav_states["angular_z"] else: self.spnav_states[self.event_mapping_to_button_mapping[ event.bnum]] = event.press
def process_spnav_events(self): event = spnav.spnav_poll_event() if event: if event.ev_type == spnav.SPNAV_EVENT_MOTION: # FIXME: Make sure these divisors are correct. Should be for most. Check linear_z, is weird self.spnav_states["linear_x"] = event.translation[0] / 350.0 self.spnav_states["linear_y"] = event.translation[2] / 350.0 self.spnav_states["linear_z"] = event.translation[1] / 350.0 self.spnav_states["angular_x"] = event.rotation[2] / 350.0 self.spnav_states["angular_y"] = event.rotation[0] / 350.0 self.spnav_states["angular_z"] = event.rotation[1] / 350.0 # print "x", self.spnav_states["linear_x"], "\t", "y", self.spnav_states["linear_y"], "\t", "z", self.spnav_states["linear_z"] # print "x", self.spnav_states["angular_x"], "\t", "y", self.spnav_states["angular_y"], "\t", "z", self.spnav_states["angular_z"] else: self.spnav_states[self.event_mapping_to_button_mapping[ event.bnum]] = event.press
def loop(self): ts = 0 btn0_state = 0 btn_event = None cmd = Cmd() while True: time.sleep(0.01) cmd.reset() #spnav.spnav_remove_events(spnav.SPNAV_EVENT_ANY) # seems broken event = spnav.spnav_poll_event() if event: if type(event) is spnav.SpnavButtonEvent: btn_event = event if event.bnum == 0: btn0_state = event.press elif type(event) is spnav.SpnavMotionEvent: if abs(event.translation[0]) > 30: cmd.y = event.translation[0] / self.lin_coef if abs(event.translation[1]) > 30: cmd.z = -1 * event.translation[1] / self.lin_coef if abs(event.translation[2]) > 30: cmd.x = event.translation[2] / self.lin_coef if abs(event.rotation[0]) > 20: cmd.ry = event.rotation[0] / self.lin_coef if abs(event.rotation[1]) > 20: cmd.rz = -1 * event.rotation[1] / self.lin_coef if abs(event.rotation[2]) > 20: cmd.rx = event.rotation[2] / self.lin_coef if (time.time() - ts) > 0.12: ts = time.time() speeds = cmd.get_speeds() if btn0_state: self.robot.speedl_tool(speeds, acc=0.10, min_time=2) else: self.robot.speedl(speeds, acc=0.10, min_time=2) btn_event = None speeds = cmd.get_speeds() #if speeds != [0 for _ in speeds]: print(event) print("Sending", speeds)
def run(self): """ Main Loop """ ret = None while ret == None: for event in pygame.event.get(): if event.type == pygame.QUIT: ret = pygame.QUIT continue spnav_event = spnav.spnav_poll_event() if spnav_event is not None: if isinstance(spnav_event, spnav.SpnavMotionEvent): evt = { 'period': spnav_event.period, 'rotation': spnav_event.rotation, 'translation': spnav_event.translation } self.old_evt = self.evt self.evt = evt self.old_delta = self.delta self.delta['period'] = evt['period'] try: self.delta['period'] = self.delta['period'] / self.old_delta['period'] except: pass self.delta['x'] += evt['rotation'][0] * self.old_delta['period'] self.delta['y'] += evt['rotation'][1] * self.old_delta['period'] self.delta['z'] += evt['rotation'][2] * self.old_delta['period'] spnav.spnav_remove_events(spnav.SPNAV_EVENT_MOTION) elif isinstance(spnav_event, spnav.SpnavButtonEvent): ret = spnav.SpnavButtonEvent continue self.clock.tick(50) self.screen.fill((0,32,0)) # It will hold transformed vertices. t = [] for v in self.vertices: # Rotate the point around X axis, then around Y axis, and finally around Z axis. r = v.rotateX(self.delta['x']).rotateY(self.delta['y']).rotateZ(self.delta['z']) # Transform the point from 3D to 2D p = r.project(self.screen.get_width(), self.screen.get_height(), 256, 4) # Put the point in the list of transformed vertices t.append(p) # Calculate the average Z values of each face. avg_z = [] i = 0 for f in self.faces: z = (t[f[0]].z + t[f[1]].z + t[f[2]].z + t[f[3]].z) / 4.0 avg_z.append([i,z]) i = i + 1 # Draw the faces using the Painter's algorithm: # Distant faces are drawn before the closer ones. for tmp in sorted(avg_z,key=itemgetter(1),reverse=True): face_index = tmp[0] f = self.faces[face_index] pointlist = [(t[f[0]].x, t[f[0]].y), (t[f[1]].x, t[f[1]].y), (t[f[1]].x, t[f[1]].y), (t[f[2]].x, t[f[2]].y), (t[f[2]].x, t[f[2]].y), (t[f[3]].x, t[f[3]].y), (t[f[3]].x, t[f[3]].y), (t[f[0]].x, t[f[0]].y)] pygame.draw.polygon(self.screen,self.colors[face_index], pointlist) pygame.display.flip() # gracefully exit spnav.spnav_close() if ret == pygame.QUIT: print('Closed the window?') pass if ret == spnav.SpnavButtonEvent: print('Clicked button to exit.') pass pygame.quit() sys.exit()
from spnav import spnav_open, spnav_poll_event, spnav_close if __name__ == '__main__': spnav_open() try: while True: event = spnav_poll_event() if event is not None: print event except KeyboardInterrupt: print '\nQuitting...' finally: spnav_close()
def test_spnav_poll_event_none(): m = mock_libspnav({'spnav_poll_event': 0}) assert spnav.spnav_poll_event() is None
def poll_loop(): event = spnav_poll_event() if event is not None: if event.ev_type == SPNAV_EVENT_MOTION: process_spnav_evnt(event)