def save_input(name):
print "save input", len(bTmAry.transforms), len(cTsAry.transforms)
Tcsv = np.array([]).reshape((-1, 14))
for M, S in zip(bTmAry.transforms, cTsAry.transforms):
btm = tflib.fromRTtoVec(tflib.toRT(M))
cts = tflib.fromRTtoVec(tflib.toRT(S))
alin = np.hstack((btm, cts))
Tcsv = np.vstack((Tcsv, alin))
np.savetxt(name, Tcsv)
return
def save_result(name):
Tcsv=np.array([]).reshape((-1,7))
for M,S in zip(bTmAry.transforms,cTsAry.transforms):
cTs=tflib.toRT(S)
if Config["mount_frame_id"]=="world":
mTb=tflib.toRT(M).I
xTs=tflib.fromRTtoVec(np.dot(np.dot(mTb,mTc),cTs))
else:
bTm=tflib.toRT(M)
xTs=tflib.fromRTtoVec(np.dot(np.dot(bTm,mTc),cTs))
Tcsv=np.vstack((Tcsv,xTs))
Tn=map(np.linalg.norm,Tcsv.T[:3].T)
err=Float64(); err.data=max(Tn)-min(Tn); pb_err.publish(err)
print "Calibration error:",err
np.savetxt(name,Tcsv)
return
def getRT(base, ref):
try:
ts = tfBuffer.lookup_transform(base, ref, rospy.Time())
RT = tflib.toRT(ts.transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
RT = None
return RT
def cb_path(msg):
global Path
Path = []
for p in msg.poses:
vec = [
p.position.x, p.position.y, p.position.z, p.orientation.x,
p.orientation.y, p.orientation.z, p.orientation.w
]
Path.append(tflib.toRT(tflib.fromVec(vec)))
def getRT(base, ref):
try:
ts = tfBuffer.lookup_transform(base, ref, rospy.Time())
rospy.loginfo("cropper::getRT::TF lookup success " + base + "->" + ref)
RT = tflib.toRT(ts.transform)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
RT = None
return RT
if Config["mount_frame_id"] == "world": #fixed camera
mTbAry = TransformArray()
for tf in bTmAry.transforms:
mTbAry.transforms.append(tflib.inv(tf))
req.world_effector = mTbAry
print "calib fixed"
else:
req.world_effector = bTmAry
print "calib handeye"
try:
res = calibrator(req)
print res.effector_camera
set_param_tf(
Config["config_tf"] + "/" + Config["camera_frame_id"] +
"/transform", res.effector_camera)
mTc = tflib.toRT(res.effector_camera)
pb_msg.publish("rcalib::visp solver success")
except rospy.ServiceException, e:
print 'Visp call failed:' + e
pb_msg.publish("rcalib::visp solver failed")
return
def cb_X2(f):
global cTsAry, bTmAry
save_input('rcalib_input.txt')
call_visp()
save_result('rcalib_result.txt')
return
}
Refs = {}
tr1 = Transform()
tr1.rotation.w = np.sqrt(2) / 2
tr1.rotation.y = np.sqrt(2) / 2
color = (0.1, 0.1, 0.1)
while not rospy.is_shutdown():
rospy.Rate(5).sleep() #1 Hz
try:
Param.update(rospy.get_param("/cutter"))
except Exception as e:
pass
try:
Refs = rospy.get_param(Param["refs"])
except Exception as e:
pass
tr1.translation.x = Param["base"] if "base" not in Refs else Refs["base"]
tr1.translation.z = Param["distance"] if "distance" not in Refs else Refs[
"distance"]
span = Param["span"] if "span" not in Refs else Refs["span"]
T1 = tflib.toRT(tr1)
markers.publishPlane(T1, span, span, color,
1.0) # pose, color, scale, lifetime
tr2 = copy.copy(tr1)
offset = Param["offset"] if "offset" not in Refs else Refs["offset"]
tr2.translation.x = tr2.translation.x + offset
T2 = tflib.toRT(tr2)
markers.publishPlane(T2, span, span, color, 1.0)
