def qualisys_info(): """ Qualisys data listener. @param qs_body: """ stop = False def signal_handler(signal, frame): stop = True sys.exit(0) signal.signal(signal.SIGINT, signal_handler) Qs = mocap.Mocap(info=1) bodies = Qs.find_available_bodies(printinfo=1) #pick the first valid body id_body = Qs.get_id_from_name("Iris2") body = mocap.Body(Qs, id_body) pose = body.getPose() qsid = pose['id'] qsx = pose['x'] qsy = pose['y'] qsyaw = pose['yaw'] qualisys_pos(qsid, qsx, qsy, qsyaw)
def on_enablemocapbutton_clicked(self, checked): """ Slot documentation goes here. """ if checked: if self.simulationCheckBox.isChecked(): try: self.s_mocap = socket.socket(socket.AF_INET, socket.SOCK_STREAM) except socket.error: print('Failed to create mocap socket') sys.exit() self.s_mocap.settimeout(5) self.s_mocap.connect((self.host, self.port_mocap)) self.mocapPoll.register(self.s_mocap, select.POLLOUT) print("Mocap Socket connected") self.mocapconnection = mocapSIM.MocapSIM( self.s_mocap) # set starting position to node 1 print('Simulation active') self.mocaplooptimer.start( int(1. / self.mocapspinbox.value() * 1000.) ) # start timer for periodic execution of mocapupdate function self.last_mocapupdate = time.time() else: try: self.mocapconnection = mocap.Mocap( self.bodynumberspinbox.value()) self.mocaplooptimer.start( int(1. / self.mocapspinbox.value() * 1000.) ) # start timer for periodic execution of mocapupdate function self.last_mocapupdate = time.time() except: self.enablemocapbutton.setChecked(False) self.logbrowser.append('Could not connect to Mocap!') return else: if self.simulationCheckBox.isChecked(): self.mocapPoll.unregister(self.s_mocap) self.s_mocap.close() print('MOCAP socket closed') self.mocapconnection.close() self.mocaplooptimer.stop() # set several buttons according to the state of the 'Mocap On' button to enable or disable them self.mocapActive = checked self.logbutton.setEnabled(checked) self.usecurposbutton.setEnabled(checked) if self.mocapActive == False and self.controlActive == False: self.simulationCheckBox.setEnabled(True) else: self.simulationCheckBox.setEnabled(False)
while not rospy.is_shutdown(): delta_time = timer.get_time_diff() for i in range(0, len(body_array)): mocap_data = Get_Body_Data(body_array[i]) mocap_data_derived = Get_Derived_Data(mocap_data, mocap_past_data[i], delta_time) #update past mocap data mocap_past_data[i] = mocap_data_derived #Publish data on topic topics_publisher[i].publish(mocap_data_derived) rate.sleep() if __name__ == "__main__": rospy.init_node("ros_mocap_unfiltered") Qs = mocap.Mocap(info=0) bodies = Qs.get_updated_bodies() if (bodies == "off"): rospy.logerr("No connection to the Qualisys Motion Capture System") sys.exit() else: start_publishing() #EOF