def main(argv):
global posearray_pub, self_ID, tranStampedArray_sets, poseStampedArray_sets, poseStampedArray_locks, LooptransArray_sets, LooptransArray_locks, posearray_pub_test, pose_with_image_pub, map_pub, withmap, br, listener
opts, args = getopt.getopt(argv, "i:t")
for opt, arg in opts:
if opt in ("-i"):
self_ID = int(arg)
if opt in ("-t"):
withmap = False
tranStampedArray_sets[str(self_ID)] = TransformStampedArray()
tranStampedArray_locks[str(self_ID)] = threading.Lock()
poseStampedArray_sets[str(self_ID)] = PoseStampedArray()
poseStampedArray_locks[str(self_ID)] = threading.Lock()
LooptransArray_sets[str(self_ID)] = TransformStampedArray()
LooptransArray_locks[str(self_ID)] = threading.Lock()
signal.signal(signal.SIGINT, trackutils.quit)
signal.signal(signal.SIGTERM, trackutils.quit)
rospy.init_node('generate_track_py', anonymous=True)
rospy.Subscriber("relpose",
TransformStamped_with_image,
self_track_pose_cb,
queue_size=100)
# srvhandle = rospy.Service('/robot{}/posearray_srv'.format(self_ID), posearray_srv, handle_posearray_srv)
srvhandle = rospy.Service('/robot{}/transarray_srv'.format(self_ID),
transarray_srv, handle_transarray_srv)
rospy.Subscriber("looppose", TransformStamped, callback_loop)
# rospy.Subscriber("/robot{}/loopinterpose".format(self_ID), PoseStampedArray, interposecallback )
# rospy.Subscriber("/robot{}/loopintertrans".format(self_ID), TransformStampedArray, intertranscallback )
posearray_pub = rospy.Publisher("posearray", PoseArray, queue_size=3)
posearray_pub_test = rospy.Publisher(
"/robot{}/posearray_test".format(self_ID), PoseArray, queue_size=3)
pose_with_image_pub = rospy.Publisher("pose_with_image",
Pose_with_image,
queue_size=3)
map_pub = rospy.Publisher("/robot{}/octomap_in".format(self_ID),
Octomap,
queue_size=3)
listener = tf.TransformListener()
maintain_tf_tree()
backt = threading.Thread(target=BackendThread)
self_tran_t = threading.Thread(target=process_self)
backt.setDaemon(True)
self_tran_t.setDaemon(True)
backt.start()
self_tran_t.start()
rospy.spin()
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 valid_loop(poseStampedArray_sets, tranStampedArray):
valid_loop_array = TransformStampedArray()
for transinfo in tranStampedArray.transformArray:
robot_current_ID = int(int(transinfo.header.frame_id) / 1e8)
frame_current_ID = int(transinfo.header.frame_id)
robot_child_ID = int(int(transinfo.child_frame_id) / 1e8)
frame_child_ID = int(transinfo.child_frame_id)
if frame_current_ID > int(poseStampedArray_sets[str(
robot_current_ID)].poseArray[-1].header.frame_id):
continue
if frame_child_ID > int(poseStampedArray_sets[str(
robot_child_ID)].poseArray[-1].header.frame_id):
continue
valid_loop_array.transformArray.append(transinfo)
return valid_loop_array
def BackendOpt(transArray, poseStampedArray, LooptransArray, posearray_pub):
Max_id = int(poseStampedArray.poseArray[-1].header.frame_id)
for loopconstrain in LooptransArray.transformArray:
ValidLooptransArray = TransformStampedArray()
if (int(loopconstrain.child_frame_id) < Max_id) and (int(
loopconstrain.header.frame_id) < Max_id):
ValidLooptransArray.transformArray.append(loopconstrain)
if not len(ValidLooptransArray.transformArray) > 0:
return
trackutils.PoseStampedarray2G2O("./tem12.g2o", poseStampedArray)
trackutils.AddTransArray2G2O("./tem12.g2o", transArray)
trackutils.AddTransArray2G2O("./tem12.g2o", LooptransArray)
poseStampedArray.poseArray = []
trackutils.gtsamOpt2PoseStampedarray("./tem12.g2o", poseStampedArray)
posearray = PoseArray()
posearray.header.frame_id = "map0"
def callback(data):
global representors_a1,looptrans_name_pub,looptrans_pub
transarray = TransformStampedArray()
imageHeader = data.imageHeader
imageFrameID = imageHeader.frame_id
representor = data.representor
matchresults = matchrepresentor (representor, representors_a1)
# print ("matchresults")
# print (matchresults)
# print ("np.array(representor)")
# print (np.array(representor))
representors_a1 = np.vstack((representors_a1 , np.array(representor) ) )
frameIDstr.append(imageFrameID)
if ( len(matchresults) > 30):
# print ("array > 30")
validresults = matchresults[0:-30]
validmatch = validresults>0.95
if ( validmatch.any() ):
# print ("array valid")
maxargs = np.argmax(validresults)
# match_relpose = TransformStamped()
# match_relpose.header.frame_id = imageFrameID
# match_relpose.child_frame_id = frameIDstr[maxargs]
# match_relpose.transform.rotation.w = 1
# match_relpose.transform.rotation.x = 0
# match_relpose.transform.rotation.y = 0
# match_relpose.transform.rotation.z = 0
# match_relpose.transform.translation.x = 0
# match_relpose.transform.translation.y = 0
# match_relpose.transform.translation.z = 0
# transarray.transformArray.append(match_relpose)
matchpair = "{id1} {id2}".format(id1 = frameIDstr[maxargs], id2 = imageFrameID)
# looptrans_pub.publish(match_relpose) # to publish relpose
print (matchpair)
looptrans_name_pub.publish(matchpair) # to publish rel fnames
tmp = 1
tmp = 1
from cv_bridge import CvBridge, CvBridgeError
import cv2
import trackutils
import time
import sys
import signal
import threading
###################
# added by jianming
import time
from time_profiling_node_prevar import TIME_PROFILING
from time_profiling_node_prevar import TIME_PROFILING_PATH
###################
transArray = TransformStampedArray()
LooptransArray = TransformStampedArray()
poseStampedArray = PoseStampedArray()
posearray_pub = None
pose_with_image_pub = None
poseStampedArraylock = threading.Lock()
br = tf.TransformBroadcaster()
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)
def BackendOpt():
global posearray_pub, transArray, poseStampedArray_sets, poseStampedArray_locks, LooptransArray_sets, LooptransArray_locks, tranStampedArray_sets, tranStampedArray_locks, BackendRunning, NewLoop, self_ID, count_save, posearray_pub_test
print("BackendOpt..........")
# if len(LooptransArray_sets[str(3-self_ID)].transformArray) > 0:
# # print(LooptransArray_sets)
print("BackendOpt..........valid")
poseStampedArray_locks[str(self_ID)].acquire()
for key, _ in LooptransArray_sets.items():
LooptransArray_locks[key].acquire()
for key, _ in tranStampedArray_sets.items():
tranStampedArray_locks[key].acquire()
# dumpname = '/tmp/data_robot{}_{}.pkl'.format(self_ID, count_save)
# with open(dumpname, 'wb') as dumpoutfile:
# pickle.dump(LooptransArray_sets, dumpoutfile)
# pickle.dump(poseStampedArray_sets, dumpoutfile)
# pickle.dump(tranStampedArray_sets, dumpoutfile)
# count_save += 1
# print("===================================LooptransArray_sets=======================================")
# print( LooptransArray_sets )
# print("===================================poseStampedArray_sets=======================================")
# print( poseStampedArray_sets)
# print("===================================tranStampedArray_sets=======================================")
# print( tranStampedArray_sets)
# print("===============================================================================================")
# count_save += 1
for key, _ in LooptransArray_sets.items():
LooptransArray_locks[key].release()
for key, _ in tranStampedArray_sets.items():
tranStampedArray_locks[key].release()
g2ofilename = "/tmp/g2o_robot{}_test.g2o".format(self_ID)
with open(g2ofilename, "w") as filetmp:
# pass
print("show first_add_node_str")
first_add_node_str = "VERTEX_SE3:QUAT {pointID} {tx} {ty} {tz} {rx} {ry} {rz} {rw}".format(
pointID=0, tx=0, ty=0, tz=0, rx=0, ry=0, rz=0, rw=1)
print(first_add_node_str)
print >> filetmp, first_add_node_str
print("shown first_add_node_str")
#add pose 和 trans
for key, looparray in LooptransArray_sets.items():
if not (int(key) == self_ID):
if not (tranStampedArray_locks.has_key(key)):
tranStampedArray_locks[key] = threading.Lock()
tranStampedArray_sets[key] = TransformStampedArray()
tranStampedArray_locks[key].acquire()
LooptransArray_locks[key].acquire()
if len(LooptransArray_sets[key].transformArray) > 0 and len(
tranStampedArray_sets[key].transformArray) > 0:
posearray_target = PoseStampedArray()
trackutils.AccReltrans2PoseStampedArray(
tranStampedArray_sets[key], posearray_target)
target_init_pose = trackutils.LoopPosearrayInitpose(
LooptransArray_sets[key].transformArray[0],
poseStampedArray_sets[str(self_ID)], posearray_target)
if not target_init_pose:
NewLoop = True
break
if not poseStampedArray_sets.has_key(key):
poseStampedArray_locks[key] = threading.Lock()
poseStampedArray_locks[key].acquire()
poseStampedArray_sets[key] = trackutils.RotatePoserray(
target_init_pose, posearray_target)
valid_loop_array = trackutils.valid_loop(
poseStampedArray_sets, LooptransArray_sets[key])
if len(valid_loop_array.transformArray) > 0:
trackutils.PoseStampedarray2G2O(g2ofilename,
poseStampedArray_sets[key],
True)
valid_loop_array = trackutils.valid_loop(
poseStampedArray_sets, tranStampedArray_sets[key])
trackutils.AddTransArray2G2O(g2ofilename, valid_loop_array)
poseStampedArray_locks[key].release()
LooptransArray_locks[key].release()
tranStampedArray_locks[key].release()
else: #ID 相等是自己 key == self_ID
tranStampedArray_locks[key].acquire()
trackutils.PoseStampedarray2G2O(g2ofilename,
poseStampedArray_sets[key], True)
with open(g2ofilename, "a") as g2ofile:
print("show cons_str")
cons_str = "EDGE_SE3:QUAT {id1} {id2} {tx} {ty} {tz} {rx} {ry} {rz} {rw} {COVAR_STR}".format(
id1=0,
id2=poseStampedArray_sets[key].poseArray[0].header.
frame_id,
tx=0,
ty=0,
tz=0,
rx=0,
ry=0,
rz=0,
rw=1,
COVAR_STR=trackutils.COVAR_STR)
print(cons_str)
print >> g2ofile, cons_str
print("shown cons_str")
valid_loop_array = trackutils.valid_loop(
poseStampedArray_sets, tranStampedArray_sets[key])
trackutils.AddTransArray2G2O(g2ofilename, valid_loop_array)
tranStampedArray_locks[key].release()
for key, looparray in LooptransArray_sets.items():
if not (int(key) == self_ID):
tranStampedArray_locks[key].acquire()
if len(LooptransArray_sets[key].transformArray) > 0 and len(
tranStampedArray_sets[key].transformArray) > 0:
valid_loop_array = trackutils.valid_loop(
poseStampedArray_sets, LooptransArray_sets[key])
if len(valid_loop_array.transformArray) > 0:
valid_loop_array = trackutils.valid_loop(
poseStampedArray_sets, LooptransArray_sets[key])
trackutils.AddTransArray2G2O(g2ofilename, valid_loop_array)
tranStampedArray_locks[key].release()
else:
tranStampedArray_locks[key].acquire()
valid_loop_array = trackutils.valid_loop(poseStampedArray_sets,
LooptransArray_sets[key])
trackutils.AddTransArray2G2O(g2ofilename, valid_loop_array)
tranStampedArray_locks[key].release()
print("start Backend Optimization")
poseStampedArray_sets_result = {}
trackutils.gtsamOpt2PoseStampedArraySet(g2ofilename,
poseStampedArray_sets_result)
for key, _ in poseStampedArray_sets_result.items():
poseStampedArray_sets[key] = poseStampedArray_sets_result[key]
print("finish Backend Optimization")
# posearray = PoseArray()
# posearray.header.frame_id = "map{}".format(self_ID)
# # poseStampedArray_locks[str(3-self_ID)].acquire()
# trackutils.StampedArray2PoseArray(poseStampedArray_sets[str(3-self_ID)], posearray)
# posearray_pub_test.publish(posearray)
poseStampedArray_locks[str(self_ID)].release()