0]
msg.translation.y = trans.transform.translation.y - pre_position[
1]
msg.translation.z = trans.transform.translation.z - pre_position[
2]
msg = InBound(msg, limits)
print(msg.translation)
vrb.pub.publish(msg)
# pre_position[0]=trans.transform.translation.x
# pre_position[1]=trans.transform.translation.y
# pre_position[2]=trans.transform.translation.z
R = kdl.Quaternion(trans_ori.transform.rotation.x,
trans_ori.transform.rotation.y,
trans_ori.transform.rotation.z,
trans_ori.transform.rotation.w)
rotated_offset = R * controller_offset
pre_position[
0] = trans.transform.translation.x + rotated_offset
pre_position[1] = trans.transform.translation.y
pre_position[2] = trans.transform.translation.z
temp_flag = vrb.offset_flag
if vrb_orientation.offset_flag == 1:
msg_ori.rotation.x = trans_ori.transform.rotation.x
msg_ori.rotation.y = trans_ori.transform.rotation.y
msg_ori.rotation.z = trans_ori.transform.rotation.z
