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