def check_obst(e): global scnPn q=xyz2quat(e) # print 'base2work quat',e i=np.linalg.inv(tflib.toRT(q)) # print 'base2work inv rt',i P=np.vstack((scnPn.T,np.ones((1,len(scnPn))))) tp=np.dot(i[:3],P).T # print "work2base points",tp.shape,tp W=np.where(tp.T[0]>=-6.0) tp=tp[W[len(W)-1]] W=np.where(tp.T[0]<=+6.0) tp=tp[W[len(W)-1]] W=np.where(tp.T[1]>=-6.0) tp=tp[W[len(W)-1]] W=np.where(tp.T[1]<=+6.0) tp=tp[W[len(W)-1]] W=np.where(tp.T[2]>=+8.0) tp=tp[W[len(W)-1]] W=np.where(tp.T[2]<=+50.0) tp=tp[W[len(W)-1]] # d=tp.astype(np.float32) # cv2.ppf_match_3d.writePLY(d,'obs.ply') print "tp",len(tp),tp.shape return True if (len(tp)<100) else False
def cb_scene_tf(tf): global bTmCurrent print "cb_scene_tf called!" bTmCurrent=tflib.toRT(tf) print 'cb_scene_tf bTmCurrent:\n', bTmCurrent return
def getRT(base, ref):
try:
ts = tfBuffer.lookup_transform(base, ref, rospy.Time())
pub_msg.publish("cropper::getRT::TF lookup success " + base + "->" +
ref)
RT = tflib.toRT(ts.transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
pub_msg.publish("cropper::getRT::TF lookup failure " + base + "->" +
ref)
RT = None
return RT
def cb_do(msg):
global RT
try:
tf = tfBuffer.lookup_transform("camera/capture0",
"camera/capture0/solve0", rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
print "tf look up failed"
f = Bool()
pub_done.publish(f)
return
else:
if RT is None:
RT = tflib.toRT(tf.transform)
rt = tflib.toRT(tf.transform)
dist = np.linalg.norm(RT[:3, 3] - rt[:3, 3])
f = Bool()
f.data = True
pub_done.publish(f)
pub_msg.publish("dist=" + str(dist))
print tf.transform
RT = rt
pub_Y1=rospy.Publisher('/robot/Y1',Bool,queue_size=1) # X1 done
pub_Y2=rospy.Publisher('/robot/Y2',Bool,queue_size=1) # X2 done RT から tfへの変換はsocket(js)側で行う
pub_Y3=rospy.Publisher('/robot/Y3',Bool,queue_size=1) # X3 done
pub_RT=rospy.Publisher('/robot/result_rt',numpy_msg(Floats),queue_size=1) # X2 done RTをsocketに通知
pub_msg=rospy.Publisher('/solver/message',String,queue_size=1) # 処理結果メッセージを通知(socket及びrqt_param_manager)
pub_err=rospy.Publisher('/solver/error',String,queue_size=1) # error_codeを通知(socket及びrqt_param_manager)
###Transform
bTmLat=np.eye(4).astype(float) #robot RT when captured
bTm=np.eye(4).astype(float)
cTs=np.eye(4,dtype=float)
bTc=np.eye(4,dtype=float)
bTmCurrent=np.eye(4).astype(float) #robot RT when captured
###Robot Calibration Result
mTc=tflib.toRT(tflib.dict2tf(rospy.get_param('/robot/calib/mTc'))) # arM tip To Camera
#print "mTc=", mTc
###Globals
recipe_id = '' # レシピID
master_ply = '' # マスター点群ファイル名
master_rt = '' # マスターRTファイル名
master_param = '' # マスターパラメータファイル名
rovi_param = '' # バックアップパラメータファイル名(/rovi以下のパラメータをバックアップしている)
rcalib_param = '' # キャリブパラメータファイル名(HOME/.ros/rcalib.yamlをバックアップしている)
error_code = 0 # error code(0が正常)
proc_message = '' # 処理メッセージ
dateStr = '' # デバッグ表示用(日時表示用)
fromUI = False # UIから呼ばれた場合はTrue,ロボットから呼ばれた場合はFalse (UIから呼ばれた場合にロボットに応答しないためのフラグ)
###Initialize