class ForceFromGravity(object):
def __init__(self):
self.tf_l = TransformListener()
self.last_wrench = None
self.wrench_sub = rospy.Subscriber('/left_wrist_ft',
WrenchStamped,
self.wrench_cb,
queue_size=1)
def wrench_cb(self, data):
self.last_wrench = data
def compute_forces(self):
qs = self.get_orientation()
o = qs.quaternion
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("wrist_left_ft_link rpy vs world: " + str( (round(r, 3), round(p, 3), round(y, 3)) ))
rospy.loginfo("in degrees: " + str( (round(degrees(r), 3), round(degrees(p), 3), round(degrees(y), 3)) ))
hand_total_force = 10 # 10 Newton, near to a Kg
fx = hand_total_force * cos(r) * -1.0 # hack
fy = hand_total_force * cos(p)
fz = hand_total_force * cos(y)
#total = abs(fx) + abs(fy) + abs(fz)
#rospy.loginfo("fx, fy, fz, total:")
#rospy.loginfo( str( (round(fx, 3), round(fy, 3), round(fz, 3), round(total, 3)) ) )
return fx, fy, fz
def get_last_forces(self):
f = self.last_wrench.wrench.force
return f.x, f.y, f.z
def get_orientation(self, from_frame="wrist_left_ft_link"):
qs = QuaternionStamped()
qs.quaternion.w = 1.0
qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
qs.header.frame_id = "/" + from_frame
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_q = self.tf_l.transformQuaternion("/world", qs)
transform_ok = True
return world_q
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" + str(e) + ")")
rospy.sleep(0.05)
qs.header.stamp = self.tf_l.getLatestCommonTime(
"world", from_frame)
def run(self):
r = rospy.Rate(1)
while not rospy.is_shutdown():
cx, cy, cz = self.compute_forces()
c_total = abs(cx) + abs(cy) + abs(cz)
fx, fy, fz = self.get_last_forces()
f_total = abs(fx) + abs(fy) + abs(fz)
rospy.loginfo("Computed Forces:" + str(round(c_total, 3)) + "\n" + str( (round(cx, 3), round(cy, 3), round(cz, 3) )))
rospy.loginfo("Real Forces :" + str(round(f_total, 3)) + "\n" + str( (round(fx, 3), round(fy, 3), round(fz, 3) )))
r.sleep()
class Predictor(object):
def __init__(self):
rospy.loginfo("Initializing biternion predictor")
self.counter = 0
modelname = rospy.get_param("~model", "head_50_50")
weightsname = abspath(expanduser(rospy.get_param("~weights", ".")))
rospy.loginfo("Predicting using {} & {}".format(modelname, weightsname))
topic = rospy.get_param("~topic", "/biternion")
self.pub = rospy.Publisher(topic, HeadOrientations, queue_size=3)
self.pub_vis = rospy.Publisher(topic + '/image', ROSImage, queue_size=3)
self.pub_pa = rospy.Publisher(topic + "/pose", PoseArray, queue_size=3)
self.pub_tracks = rospy.Publisher(topic + "/tracks", TrackedPersons, queue_size=3)
# Ugly workaround for "jumps back in time" that the synchronizer sometime does.
self.last_stamp = rospy.Time()
# Create and load the network.
netlib = import_module(modelname)
self.net = netlib.mknet()
self.net.__setstate__(np.load(weightsname))
self.net.evaluate()
self.aug = netlib.mkaug(None, None)
self.preproc = netlib.preproc
self.getrect = netlib.getrect
# Do a fake forward-pass for precompilation.
im = cutout(np.zeros((480,640,3), np.uint8), 0, 0, 150, 450)
im = next(self.aug.augimg_pred(self.preproc(im), fast=True))
self.net.forward(np.array([im]))
rospy.loginfo("BiternionNet initialized")
src = rospy.get_param("~src", "tra")
subs = []
if src == "tra":
subs.append(message_filters.Subscriber(rospy.get_param("~tra", "/TODO"), TrackedPersons2d))
elif src == "ubd":
subs.append(message_filters.Subscriber(rospy.get_param("~ubd", "/upper_body_detector/detections"), UpperBodyDetector))
else:
raise ValueError("Unknown source type: " + src)
rgb = rospy.get_param("~rgb", "/head_xtion/rgb/image_rect_color")
subs.append(message_filters.Subscriber(rgb, ROSImage))
subs.append(message_filters.Subscriber(rospy.get_param("~d", "/head_xtion/depth/image_rect_meters"), ROSImage))
subs.append(message_filters.Subscriber('/'.join(rgb.split('/')[:-1] + ['camera_info']), CameraInfo))
tra3d = rospy.get_param("~tra3d", "")
if src == "tra" and tra3d:
subs.append(message_filters.Subscriber(tra3d, TrackedPersons))
self.listener = TransformListener()
ts = message_filters.ApproximateTimeSynchronizer(subs, queue_size=5, slop=0.5)
ts.registerCallback(self.cb)
def cb(self, src, rgb, d, caminfo, *more):
# Ugly workaround because approximate sync sometimes jumps back in time.
if rgb.header.stamp <= self.last_stamp:
rospy.logwarn("Jump back in time detected and dropped like it's hot")
return
self.last_stamp = rgb.header.stamp
detrects = get_rects(src)
# Early-exit to minimize CPU usage if possible.
#if len(detrects) == 0:
# return
# If nobody's listening, why should we be computing?
if 0 == sum(p.get_num_connections() for p in (self.pub, self.pub_vis, self.pub_pa, self.pub_tracks)):
return
header = rgb.header
bridge = CvBridge()
rgb = bridge.imgmsg_to_cv2(rgb)[:,:,::-1] # Need to do BGR-RGB conversion manually.
d = bridge.imgmsg_to_cv2(d)
imgs = []
for detrect in detrects:
detrect = self.getrect(*detrect)
det_rgb = cutout(rgb, *detrect)
det_d = cutout(d, *detrect)
# Preprocess and stick into the minibatch.
im = subtractbg(det_rgb, det_d, 1.0, 0.5)
im = self.preproc(im)
imgs.append(im)
sys.stderr.write("\r{}".format(self.counter)) ; sys.stderr.flush()
self.counter += 1
# TODO: We could further optimize by putting all augmentations in a
# single batch and doing only one forward pass. Should be easy.
if len(detrects):
bits = [self.net.forward(batch) for batch in self.aug.augbatch_pred(np.array(imgs), fast=True)]
preds = bit2deg(ensemble_biternions(bits)) - 90 # Subtract 90 to correct for "my weird" origin.
# print(preds)
else:
preds = []
if 0 < self.pub.get_num_connections():
self.pub.publish(HeadOrientations(
header=header,
angles=list(preds),
confidences=[0.83] * len(imgs)
))
# Visualization
if 0 < self.pub_vis.get_num_connections():
rgb_vis = rgb[:,:,::-1].copy()
for detrect, alpha in zip(detrects, preds):
l, t, w, h = self.getrect(*detrect)
px = int(round(np.cos(np.deg2rad(alpha))*w/2))
py = -int(round(np.sin(np.deg2rad(alpha))*h/2))
cv2.rectangle(rgb_vis, (detrect[0], detrect[1]), (detrect[0]+detrect[2],detrect[1]+detrect[3]), (0,255,255), 1)
cv2.rectangle(rgb_vis, (l,t), (l+w,t+h), (0,255,0), 2)
cv2.line(rgb_vis, (l+w//2, t+h//2), (l+w//2+px,t+h//2+py), (0,255,0), 2)
# cv2.putText(rgb_vis, "{:.1f}".format(alpha), (l, t+25), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,0,255), 2)
vismsg = bridge.cv2_to_imgmsg(rgb_vis, encoding='rgb8')
vismsg.header = header # TODO: Seems not to work!
self.pub_vis.publish(vismsg)
if 0 < self.pub_pa.get_num_connections():
fx, cx = caminfo.K[0], caminfo.K[2]
fy, cy = caminfo.K[4], caminfo.K[5]
poseArray = PoseArray(header=header)
for (dx, dy, dw, dh, dd), alpha in zip(get_rects(src, with_depth=True), preds):
dx, dy, dw, dh = self.getrect(dx, dy, dw, dh)
# PoseArray message for boundingbox centres
poseArray.poses.append(Pose(
position=Point(
x=dd*((dx+dw/2.0-cx)/fx),
y=dd*((dy+dh/2.0-cy)/fy),
z=dd
),
# TODO: Use global UP vector (0,0,1) and transform into frame used by this message.
orientation=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0]))
))
self.pub_pa.publish(poseArray)
if len(more) == 1 and 0 < self.pub_tracks.get_num_connections():
t3d = more[0]
try:
self.listener.waitForTransform(header.frame_id, t3d.header.frame_id, rospy.Time(), rospy.Duration(1))
for track, alpha in zip(t3d.tracks, preds):
track.pose.pose.orientation = self.listener.transformQuaternion(t3d.header.frame_id, QuaternionStamped(
header=header,
# TODO: Same as above!
quaternion=Quaternion(*quaternion_about_axis(np.deg2rad(alpha), [0, -1, 0]))
)).quaternion
self.pub_tracks.publish(t3d)
except TFException:
pass
def transformQuaternion(self, target_frame, quat):
now = rospy.Time.now()
self.waitForTransform(target_frame, quat.header.frame_id, rospy.Time.now(), rospy.Duration(5.0))
quat.header.stamp = now
return TransformListener.transformQuaternion(self, target_frame, quat)
class Predictor(object):
def __init__(self):
rospy.loginfo("Initializing biternion predictor")
self.counter = 0
modelname = rospy.get_param("~model", "head_50_50")
weightsname = abspath(expanduser(rospy.get_param("~weights", ".")))
rospy.loginfo("Predicting using {} & {}".format(
modelname, weightsname))
topic = rospy.get_param("~topic", "/biternion")
self.pub = rospy.Publisher(topic, HeadOrientations, queue_size=3)
self.pub_vis = rospy.Publisher(topic + '/image',
ROSImage,
queue_size=3)
self.pub_pa = rospy.Publisher(topic + "/pose", PoseArray, queue_size=3)
# Ugly workaround for "jumps back in time" that the synchronizer sometime does.
self.last_stamp = rospy.Time()
# Create and load the network.
netlib = import_module(modelname)
self.net = netlib.mknet()
self.net.__setstate__(np.load(weightsname))
self.net.evaluate()
self.aug = netlib.mkaug(None, None)
self.preproc = netlib.preproc
self.getrect = netlib.getrect
# Do a fake forward-pass for precompilation.
im = cutout(np.zeros((480, 640, 3), np.uint8), 0, 0, 150, 450)
im = next(self.aug.augimg_pred(self.preproc(im), fast=True))
self.net.forward(np.array([im]))
rospy.loginfo("BiternionNet initialized")
src = rospy.get_param("~src", "tra")
subs = []
if src == "tra":
subs.append(
message_filters.Subscriber(rospy.get_param("~tra", "/TODO"),
TrackedPersons2d))
elif src == "ubd":
subs.append(
message_filters.Subscriber(
rospy.get_param("~ubd", "/upper_body_detector/detections"),
UpperBodyDetector))
else:
raise ValueError("Unknown source type: " + src)
rgb = rospy.get_param("~rgb", "/head_xtion/rgb/image_rect_color")
subs.append(message_filters.Subscriber(rgb, ROSImage))
subs.append(
message_filters.Subscriber(
rospy.get_param("~d", "/head_xtion/depth/image_rect_meters"),
ROSImage))
subs.append(
message_filters.Subscriber(
'/'.join(rgb.split('/')[:-1] + ['camera_info']), CameraInfo))
tra3d = rospy.get_param("~tra3d", "")
if src == "tra" and tra3d and HAS_TRACKED_PERSONS:
self.pub_tracks = rospy.Publisher(topic + "/tracks",
TrackedPersons,
queue_size=3)
subs.append(message_filters.Subscriber(tra3d, TrackedPersons))
self.listener = TransformListener()
else:
self.pub_tracks = None
ts = message_filters.ApproximateTimeSynchronizer(subs,
queue_size=5,
slop=0.5)
ts.registerCallback(self.cb)
def cb(self, src, rgb, d, caminfo, *more):
# Ugly workaround because approximate sync sometimes jumps back in time.
if rgb.header.stamp <= self.last_stamp:
rospy.logwarn(
"Jump back in time detected and dropped like it's hot")
return
self.last_stamp = rgb.header.stamp
detrects = get_rects(src)
# Early-exit to minimize CPU usage if possible.
#if len(detrects) == 0:
# return
# If nobody's listening, why should we be computing?
listeners = sum(p.get_num_connections()
for p in (self.pub, self.pub_vis, self.pub_pa))
if self.pub_tracks is not None:
listeners += self.pub_tracks.get_num_connections()
if listeners == 0:
return
header = rgb.header
bridge = CvBridge()
rgb = bridge.imgmsg_to_cv2(
rgb)[:, :, ::-1] # Need to do BGR-RGB conversion manually.
d = bridge.imgmsg_to_cv2(d)
imgs = []
for detrect in detrects:
detrect = self.getrect(*detrect)
det_rgb = cutout(rgb, *detrect)
det_d = cutout(d, *detrect)
# Preprocess and stick into the minibatch.
im = subtractbg(det_rgb, det_d, 1.0, 0.5)
im = self.preproc(im)
imgs.append(im)
sys.stderr.write("\r{}".format(self.counter))
sys.stderr.flush()
self.counter += 1
# TODO: We could further optimize by putting all augmentations in a
# single batch and doing only one forward pass. Should be easy.
if len(detrects):
bits = [
self.net.forward(batch)
for batch in self.aug.augbatch_pred(np.array(imgs), fast=True)
]
preds = bit2deg(ensemble_biternions(
bits)) - 90 # Subtract 90 to correct for "my weird" origin.
# print(preds)
else:
preds = []
if 0 < self.pub.get_num_connections():
self.pub.publish(
HeadOrientations(header=header,
angles=list(preds),
confidences=[0.83] * len(imgs)))
# Visualization
if 0 < self.pub_vis.get_num_connections():
rgb_vis = rgb[:, :, ::-1].copy()
for detrect, alpha in zip(detrects, preds):
l, t, w, h = self.getrect(*detrect)
px = int(round(np.cos(np.deg2rad(alpha)) * w / 2))
py = -int(round(np.sin(np.deg2rad(alpha)) * h / 2))
cv2.rectangle(
rgb_vis, (detrect[0], detrect[1]),
(detrect[0] + detrect[2], detrect[1] + detrect[3]),
(0, 255, 255), 1)
cv2.rectangle(rgb_vis, (l, t), (l + w, t + h), (0, 255, 0), 2)
cv2.line(rgb_vis, (l + w // 2, t + h // 2),
(l + w // 2 + px, t + h // 2 + py), (0, 255, 0), 2)
# cv2.putText(rgb_vis, "{:.1f}".format(alpha), (l, t+25), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,0,255), 2)
vismsg = bridge.cv2_to_imgmsg(rgb_vis, encoding='rgb8')
vismsg.header = header # TODO: Seems not to work!
self.pub_vis.publish(vismsg)
if 0 < self.pub_pa.get_num_connections():
fx, cx = caminfo.K[0], caminfo.K[2]
fy, cy = caminfo.K[4], caminfo.K[5]
poseArray = PoseArray(header=header)
for (dx, dy, dw, dh,
dd), alpha in zip(get_rects(src, with_depth=True), preds):
dx, dy, dw, dh = self.getrect(dx, dy, dw, dh)
# PoseArray message for boundingbox centres
poseArray.poses.append(
Pose(
position=Point(x=dd * ((dx + dw / 2.0 - cx) / fx),
y=dd * ((dy + dh / 2.0 - cy) / fy),
z=dd),
# TODO: Use global UP vector (0,0,1) and transform into frame used by this message.
orientation=Quaternion(*quaternion_about_axis(
np.deg2rad(alpha), [0, -1, 0]))))
self.pub_pa.publish(poseArray)
if len(
more
) == 1 and self.pub_tracks is not None and 0 < self.pub_tracks.get_num_connections(
):
t3d = more[0]
try:
self.listener.waitForTransform(header.frame_id,
t3d.header.frame_id,
rospy.Time(), rospy.Duration(1))
for track, alpha in zip(t3d.tracks, preds):
track.pose.pose.orientation = self.listener.transformQuaternion(
t3d.header.frame_id,
QuaternionStamped(
header=header,
# TODO: Same as above!
quaternion=Quaternion(*quaternion_about_axis(
np.deg2rad(alpha), [0, -1, 0])))).quaternion
self.pub_tracks.publish(t3d)
except TFException:
pass
def transformQuaternion(self, target_frame, quat):
now = rospy.Time.now()
self.waitForTransform(target_frame, quat.header.frame_id,
rospy.Time.now(), rospy.Duration(5.0))
quat.header.stamp = now
return TransformListener.transformQuaternion(self, target_frame, quat)
class ForceFromGravity(object):
def __init__(self):
self.tf_l = TransformListener()
self.last_wrench = None
self.wrench_sub = rospy.Subscriber('/left_wrist_ft',
WrenchStamped,
self.wrench_cb,
queue_size=1)
def wrench_cb(self, data):
self.last_wrench = data
def compute_forces(self):
qs = self.get_orientation()
o = qs.quaternion
r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
rospy.loginfo("wrist_left_ft_link rpy vs world: " +
str((round(r, 3), round(p, 3), round(y, 3))))
rospy.loginfo("in degrees: " +
str((round(degrees(r), 3), round(degrees(p), 3),
round(degrees(y), 3))))
hand_total_force = 10 # 10 Newton, near to a Kg
fx = hand_total_force * cos(r) * -1.0 # hack
fy = hand_total_force * cos(p)
fz = hand_total_force * cos(y)
#total = abs(fx) + abs(fy) + abs(fz)
#rospy.loginfo("fx, fy, fz, total:")
#rospy.loginfo( str( (round(fx, 3), round(fy, 3), round(fz, 3), round(total, 3)) ) )
return fx, fy, fz
def get_last_forces(self):
f = self.last_wrench.wrench.force
return f.x, f.y, f.z
def get_orientation(self, from_frame="wrist_left_ft_link"):
qs = QuaternionStamped()
qs.quaternion.w = 1.0
qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
qs.header.frame_id = "/" + from_frame
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_q = self.tf_l.transformQuaternion("/world", qs)
transform_ok = True
return world_q
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" +
str(e) + ")")
rospy.sleep(0.05)
qs.header.stamp = self.tf_l.getLatestCommonTime(
"world", from_frame)
def run(self):
r = rospy.Rate(1)
while not rospy.is_shutdown():
cx, cy, cz = self.compute_forces()
c_total = abs(cx) + abs(cy) + abs(cz)
fx, fy, fz = self.get_last_forces()
f_total = abs(fx) + abs(fy) + abs(fz)
rospy.loginfo("Computed Forces:" + str(round(c_total, 3)) + "\n" +
str((round(cx, 3), round(cy, 3), round(cz, 3))))
rospy.loginfo("Real Forces :" + str(round(f_total, 3)) + "\n" +
str((round(fx, 3), round(fy, 3), round(fz, 3))))
r.sleep()
