def callback(data):
global posearray_pub, pose_with_image_pub, transArray, poseStampedArray, poseStampedArraylock, br
print(data.TF)
poseStampedArraylock.acquire()
trackutils.appendTrans2PoseStampedArray(data.TF, poseStampedArray)
trackutils.appendTrans2TransArray(data.TF, transArray)
poseStampedArraylock.release()
vo_posearray = PoseArray()
map_posearray = PoseArray()
map_posearray.header.frame_id = "map0"
trackutils.StampedArray2PoseArray(poseStampedArray, vo_posearray)
trackutils.VOPoseArray2MapPoseArray(vo_posearray, map_posearray)
posearray_pub.publish(map_posearray)
pose_with_image = Pose_with_image()
pose_with_image.header.stamp = data.TF.header.stamp
pose_with_image.header.frame_id = "/odom"
pose_with_image.pose = map_posearray.poses[-1]
pose_with_image.image = data.image
pose_with_image.depth = data.depth
pose_with_image.P = data.P
pose_with_image_pub.publish(pose_with_image)
tf_msg = TransformStamped()
tf_msg.header.seq = data.TF.header.seq
tf_msg.header.stamp = data.TF.header.stamp
tf_msg.header.frame_id = "/map0"
tf_msg.child_frame_id = "/odom"
tf_msg.transform.translation = map_posearray.poses[-1].position
tf_msg.transform.rotation = map_posearray.poses[-1].orientation
print(tf_msg)
br.sendTransformMessage(tf_msg)
def callback_loop(data):
global LooptransArray_sets, LooptransArray_locks, NewLoop
#child_frame_id表示先来的图片,或者是对方的图片。header.frame_id 表示后来的图片,或者自己的图片
if (int(int(data.child_frame_id) / 1e8) == int(
int(data.header.frame_id) / 1e8)): #如果两个位姿相同
current_robot_id = int(int(data.header.frame_id) / 1e8)
assert (current_robot_id == self_ID) #验证确实是自己的位姿
LooptransArray_locks[str(self_ID)].acquire()
trackutils.appendTrans2TransArray(data,
LooptransArray_sets[str(self_ID)],
False)
LooptransArray_locks[str(self_ID)].release()
else: #如果不是两个位姿不同
other_robot_id = int(int(data.child_frame_id) / 1e8)
self_robot_id = int(int(data.header.frame_id) / 1e8)
print("PreinterOKKKKKKKKKKKKKKKKKKKKKK")
assert (self_robot_id == self_ID) #验证确实和自己相关
print("interOKKKKKKKKKKKKKKKKKKKKKK")
if not (LooptransArray_sets.has_key(str(other_robot_id))):
LooptransArray_sets[str(other_robot_id)] = TransformStampedArray()
LooptransArray_locks[str(other_robot_id)] = threading.Lock()
LooptransArray_locks[str(other_robot_id)].acquire()
trackutils.appendTrans2TransArray(
data, LooptransArray_sets[str(other_robot_id)], False)
LooptransArray_locks[str(other_robot_id)].release()
NewLoop = True
print("NewLoop")
print(data)
def process_self():
global self_ID, self_transArray, poseStampedArray_sets, poseStampedArray_locks, posearray_pub, self_trans_queue, pose_with_image_pub
while (True):
time.sleep(0.1)
if self_trans_queue.empty():
continue
data = self_trans_queue.get()
if (int(int(data.TF.child_frame_id) / 1e8) == int(
int(data.TF.header.frame_id) / 1e8)): #如果是自己的位姿
current_robot_id = int(int(data.TF.child_frame_id) / 1e8)
print(data.TF)
assert (
current_robot_id == self_ID
), str(data.TF) + " current_robot_id: " + str(
current_robot_id) + " self_ID: " + str(self_ID) #验证确实是自己的位姿
poseStampedArray_locks[str(current_robot_id)].acquire()
trackutils.appendTrans2PoseStampedArray(
data.TF, poseStampedArray_sets[str(current_robot_id)])
trackutils.appendTrans2TransArray(
data.TF, tranStampedArray_sets[str(self_ID)])
poseStampedArray_locks[str(current_robot_id)].release()
vo_posearray = PoseArray()
map_posearray = PoseArray()
map_posearray.header.stamp = data.TF.header.stamp
map_posearray.header.frame_id = "odom{}".format(self_ID)
poseStampedArray_locks[str(current_robot_id)].acquire()
trackutils.StampedArray2PoseArray(
poseStampedArray_sets[str(current_robot_id)], vo_posearray)
poseStampedArray_locks[str(current_robot_id)].release()
trackutils.VOPoseArray2MapPoseArray(vo_posearray, map_posearray)
posearray_pub.publish(map_posearray)
pose_with_image = Pose_with_image()
pose_with_image.header.stamp = data.TF.header.stamp
pose_with_image.header.frame_id = "/scene{}".format(self_ID)
pose_with_image.pose = map_posearray.poses[-1]
pose_with_image.image = data.image
pose_with_image.depth = data.depth
pose_with_image.P = data.P
pose_with_image_pub.publish(pose_with_image)
tf_msg = TransformStamped()
tf_msg.header.stamp = data.TF.header.stamp
tf_msg.header.frame_id = "/odom{}".format(self_ID)
tf_msg.child_frame_id = "/scene{}".format(self_ID)
tf_msg.transform = trackutils.pose2trans(map_posearray.poses[-1])
# tf_msg.transform.translation = map_posearray.poses[-1].position
# tf_msg.transform.rotation = map_posearray.poses[-1].orientation
print(tf_msg)
# br.sendTransformMessage(tf_msg)
publish_tf_to_tree(tf_msg, "/odom{}".format(self_ID),
"/base_link{}".format(self_ID))
maintain_tf_tree(1, 1)
def callback(data):
if (TIME_PROFILING):
start = time.time()
global posearray_pub, pose_with_image_pub, transArray, poseStampedArray, poseStampedArraylock, br
print(data.TF)
poseStampedArraylock.acquire()
trackutils.appendTrans2PoseStampedArray(data.TF, poseStampedArray)
trackutils.appendTrans2TransArray(data.TF, transArray)
poseStampedArraylock.release()
vo_posearray = PoseArray()
map_posearray = PoseArray()
map_posearray.header.frame_id = "map0"
trackutils.StampedArray2PoseArray(poseStampedArray, vo_posearray)
trackutils.VOPoseArray2MapPoseArray(vo_posearray, map_posearray)
posearray_pub.publish(map_posearray)
pose_with_image = Pose_with_image()
pose_with_image.header.stamp = data.TF.header.stamp
pose_with_image.header.frame_id = "/odom"
pose_with_image.pose = map_posearray.poses[-1]
pose_with_image.image = data.image
pose_with_image.depth = data.depth
pose_with_image.P = data.P
pose_with_image_pub.publish(pose_with_image)
tf_msg = TransformStamped()
tf_msg.header.seq = data.TF.header.seq
tf_msg.header.stamp = data.TF.header.stamp
tf_msg.header.frame_id = "/map0"
tf_msg.child_frame_id = "/odom"
tf_msg.transform.translation = map_posearray.poses[-1].position
tf_msg.transform.rotation = map_posearray.poses[-1].orientation
print(tf_msg)
br.sendTransformMessage(tf_msg)
# tf_msg.header.frame_id = "base_footprint"
# tf_msg.child_frame_id = "odom"
# tf_msg.transform.translation = vo_posearray.poses[-1].position
# tf_msg.transform.rotation = vo_posearray.poses[-1].orientation
# print(tf_msg)
# br.sendTransformMessage(tf_msg)
if (TIME_PROFILING):
ofile = open(TIME_PROFILING_PATH + "generate_track_node_relpose.txt",
'a')
end = time.time()
ofile.write('generate_track_node_relpose time: %s Seconds' %
(end - start))
ofile.write('\n')
def callback_loop(data):
if (TIME_PROFILING):
start = time.time()
global LooptransArray, NewLoop
print("loop detected")
trackutils.appendTrans2TransArray(data, LooptransArray, False)
NewLoop = True
if (TIME_PROFILING):
ofile = open(TIME_PROFILING_PATH + "generate_track_node_looppose.txt",
'a')
end = time.time()
ofile.write('generate_track_node_looppose time: %s Seconds' %
(end - start))
ofile.write('\n')
def callback_loop(data):
global LooptransArray, NewLoop
print("loop detected")
trackutils.appendTrans2TransArray(data, LooptransArray, False)
NewLoop = True