def run(self):
i = 0
input_port = yarp.BufferedPortBottle()
input_port.open(self.portname)
input_port.useCallback(self.rec)
st = yarp.Stamp()
while not self.killswitch.is_set():
input_port.getEnvelope(st)
ts = st.getTime()
b = self.decode_q.get()
binp = event_driven.bottleToVBottle(b)
data = event_driven.getData(binp)
self.out_q.put((ts, data))
i += 1
def stream_parts(port, pose, threshold=0.5):
all_joints_names = cfg.all_joints_names
all_body_parts = port.prepare()
all_body_parts.clear()
body_parts = yarp.Bottle()
body_parts.clear()
part_bottle = yarp.Bottle()
# for pidx in range(num_joints):
part_bottle.clear()
head = ((pose[13,:]+pose[12,:])/2.0)
add_part(part_bottle, head , 'forehead') # forehead
add_part(part_bottle, pose[12, :], 'chin') # chin
add_part(part_bottle, pose[8, :], 'Rshoulder') # R shoulder
add_part(part_bottle, pose[7, :], 'Relbow') # R elbow
add_part(part_bottle, pose[6, :], 'Rwrist') # R wrist
add_part(part_bottle, pose[9, :], 'Lshoulder') # L shoulder
add_part(part_bottle, pose[10, :], 'Lelbow') # L elbow
add_part(part_bottle, pose[11, :], 'Lwrist') # L wrist
add_part(part_bottle, pose[2, :], 'Rhip') # R hip
add_part(part_bottle, pose[1, :], 'Rknee') # R knee
add_part(part_bottle, pose[0, :], 'Rankle') # R ankle
add_part(part_bottle, pose[3, :], 'Lhip') # L hip
add_part(part_bottle, pose[4, :], 'Lknee') # L knee
add_part(part_bottle, pose[5, :], 'Lankle') # L ankle
body_parts.addList().read(part_bottle)
all_body_parts.addList().read(body_parts)
ts = yarp.Stamp()
ts.update()
port.setEnvelope(ts)
port.write()
#!/usr/bin/python
import mqtt_wrapper
import yarp
import signal
from time import sleep
yarp.Network.init()
Stamp = yarp.Stamp()
acceleration_port = yarp.BufferedPortBottle()
acceleration_port.open("/imu/acceleration")
velocity_port = yarp.BufferedPortBottle()
velocity_port.open("/imu/velocity")
class imu_bridge_recordings(mqtt_wrapper.bridge):
def msg_process(self, msg):
msg_topic = msg.topic.split("/")
if(msg_topic[-1] == "imu"):
topic_name = msg_topic[0].replace(" ", "_")
msg_list = msg.payload.split(";")
topic_name_acc = "/"+ topic_name + "/acceleration"
topic_name_vel = "/"+ topic_name + "/velocity"
pkgSizeAcc = int(msg_list[0])
moduleSizeAcc = 4*pkgSizeAcc+2
acc_msg = msg_list[2:moduleSizeAcc]
def stream_parts(port, pose, threshold=0.5):
num_joints = pose.shape[0]
pose_conf = pose[:, 2]
pose = pose.astype(int)
all_body_parts = port.prepare()
all_body_parts.clear()
body_parts = yarp.Bottle()
body_parts.clear()
hands = yarp.Bottle()
hands.clear()
hands_pos = yarp.Bottle()
hands_pos.clear()
hands.addString('hands')
hands_pos.addInt(pose[6, 0])
hands_pos.addInt(pose[6, 1])
hands_pos.addInt(pose[11, 0])
hands_pos.addInt(pose[11, 1])
hands_pos.addDouble(pose_conf[6])
hands_pos.addDouble(pose_conf[11])
hands.addList().read(hands_pos)
body_parts.addList().read(hands)
elbows = yarp.Bottle()
elbows.clear()
elbows_pos = yarp.Bottle()
elbows_pos.clear()
elbows.addString('elbows')
elbows_pos.addInt(pose[7, 0])
elbows_pos.addInt(pose[7, 1])
elbows_pos.addInt(pose[10, 0])
elbows_pos.addInt(pose[10, 1])
elbows_pos.addDouble(pose_conf[7])
elbows_pos.addDouble(pose_conf[10])
elbows.addList().read(elbows_pos)
body_parts.addList().read(elbows)
shoulders = yarp.Bottle()
shoulders.clear()
shoulders_pos = yarp.Bottle()
shoulders_pos.clear()
shoulders.addString('shoulders')
shoulders_pos.addInt(pose[8, 0])
shoulders_pos.addInt(pose[8, 1])
shoulders_pos.addInt(pose[9, 0])
shoulders_pos.addInt(pose[9, 1])
shoulders_pos.addDouble(pose_conf[8])
shoulders_pos.addDouble(pose_conf[9])
shoulders.addList().read(shoulders_pos)
body_parts.addList().read(shoulders)
head = yarp.Bottle()
head.clear()
head_pos = yarp.Bottle()
head_pos.clear()
head.addString('head')
head_pos.addInt(int((pose[12, 0] + pose[13, 0]) / 2.0))
head_pos.addInt(int((pose[12, 1] + pose[13, 1]) / 2.0))
head_pos.addDouble((pose_conf[12] + pose_conf[13]) / 2.0)
head.addList().read(head_pos)
body_parts.addList().read(head)
all_body_parts.addList().read(body_parts)
ts = yarp.Stamp()
ts.update()
port.setEnvelope(ts)
port.write()
def updateModule(self):
# synchronize on rgb stream
rgb = self.rgb_in.read(True)
depth = self.depth_in.read(False)
mask = self.mask_in.read(False)
camera_pose = self.camera_pose_in.read(False)
if (rgb is not None) and (depth is not None) and (mask is not None) and (camera_pose is not None):
camera_position = numpy.array([camera_pose[0], camera_pose[1], camera_pose[2]])
camera_axis = numpy.array([camera_pose[3], camera_pose[4], camera_pose[5]])
camera_angle = camera_pose[6]
self.rgb_image.copy(rgb)
rgb_frame = numpy.frombuffer(self.rgb_buffer, dtype=numpy.uint8).reshape(self.options.height, self.options.width, 3)
self.depth_image.copy(depth)
depth_frame = numpy.frombuffer(self.depth_buffer, dtype=numpy.float32).reshape(self.options.height, self.options.width)
self.mask_image.copy(mask)
mask_frame = numpy.frombuffer(self.mask_buffer, dtype=numpy.uint8).reshape(self.options.height, self.options.width)
prediction = self.inference.inference(self.options.object_id, rgb_frame, depth_frame, mask_frame)
if len(prediction) != 0:
object_orientation = Quaternion(axis=[prediction[3], prediction[4], prediction[5]], angle = prediction[6])
camera_orientation = Quaternion(axis=camera_axis, angle=camera_angle).rotation_matrix
total_orientation = camera_orientation.dot(object_orientation.rotation_matrix)
total_orientation_axis = Quaternion(matrix=total_orientation).axis
total_orientation_angle = Quaternion(matrix=total_orientation).angle
total_position = camera_position + camera_orientation.dot(prediction[0:3])
stamp = yarp.Stamp()
self.rgb_in.getEnvelope(stamp)
new_stamp = stamp.getTime()
velocity = [0, 0, 0, 0, 0, 0]
if self.last_stamp > 0:
elapsed = new_stamp - self.last_stamp
velocity[0:3] = (total_position - self.last_total_position) / elapsed
relative_orientation = total_orientation.dot(self.last_total_orientation.transpose())
acos_argument = (relative_orientation.trace() - 1) / 2.0
if acos_argument < -1.0:
acos_argument = -1.0
else:
acos_argument = 1.0
theta = math.acos(acos_argument) + sys.float_info.min
ang_velocity_skew = 1.0 / (2.0 * elapsed) * theta / math.sin(theta) * (relative_orientation - relative_orientation.transpose())
velocity[3:6] = [-ang_velocity_skew[1, 2], ang_velocity_skew[0, 2], -ang_velocity_skew[0, 1]]
self.last_stamp = new_stamp
self.last_total_position = total_position
self.last_total_orientation = total_orientation
output_vector = yarp.Vector()
output_vector.resize(13)
output_vector[0] = total_position[0]
output_vector[1] = total_position[1]
output_vector[2] = total_position[2]
output_vector[3] = total_orientation_axis[0]
output_vector[4] = total_orientation_axis[1]
output_vector[5] = total_orientation_axis[2]
output_vector[6] = total_orientation_angle
output_vector[7] = velocity[0]
output_vector[8] = velocity[1]
output_vector[9] = velocity[2]
output_vector[10] = velocity[3]
output_vector[11] = velocity[4]
output_vector[12] = velocity[5]
self.prediction_out.write(output_vector)
# print(1.0 / (time.time() - start_time))
return True