def publish(self):
if not self._initialized:
return
now = rospy.Time.now()
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = now
g_msg.header.frame_id = 'base_link'
gv_msg = PoseArray()
gv_msg.header.stamp = now
gv_msg.header.frame_id = 'base_link'
# alignment ...
q = tx.quaternion_from_euler(np.pi / 2, 0, -np.pi / 2)
for i in range(self._num_markers):
txn = self._xyz[i]
rxn = tx.quaternion_from_euler(*(self._rpy[i]))
rxn = tx.quaternion_multiply(rxn, q)
msg = AprilTagDetection()
msg.id = [i]
msg.size = [self._tag_size]
pwcs = msg.pose
pwcs.header.frame_id = 'base_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
g_msg.detections.append(msg)
gv_msg.poses.append(pwcs.pose.pose)
self._tfb.sendTransform(txn,
rxn,
now,
child='gt_tag_{}'.format(i),
parent='base_link')
self._gpub.publish(g_msg)
self._gvpub.publish(gv_msg)
def make_msg(self, Depth1Image, Depth2Image, detection_data, camera1_param,
camera2_param):
bboxes_from_camera1 = BoundingBoxes()
bboxes_from_camera2 = BoundingBoxes()
bboxes_from_camera1.header = Depth1Image.header
bboxes_from_camera2.header = Depth2Image.header
self.now = rospy.get_rostime()
self.timebefore = detection_data.header.stamp
# Add point obstacle
#self.obstacle_msg = ObstacleArrayMsg()
self.obstacle_msg.header.stamp = detection_data.header.stamp
self.obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map
#self.marker_data = MarkerArray()
self.landmark_msg = AprilTagDetectionArray()
self.landmark_msg.detections.append(AprilTagDetection())
self.landmark_msg.header.stamp = detection_data.header.stamp
#opencvに変換
bridge = CvBridge()
try:
Depth1image = bridge.imgmsg_to_cv2(Depth1Image,
desired_encoding="passthrough")
Depth2image = bridge.imgmsg_to_cv2(Depth2Image,
desired_encoding="passthrough")
except CvBridgeError as e:
print(e)
for bbox in detection_data.bounding_boxes:
if (bbox.ymin + bbox.ymax) / 2 < Depth1image.shape[0]:
if bbox.ymax > Depth1image.shape[0]:
bbox.ymax = Depth1image.shape[0]
bboxes_from_camera1.bounding_boxes.append(bbox)
else:
bbox.ymin = bbox.ymin - Depth1image.shape[0]
bbox.ymax = bbox.ymax - Depth1image.shape[0]
if bbox.ymin < 0:
bbox.ymin = 0
bboxes_from_camera2.bounding_boxes.append(bbox)
camera1_obstacle_msg, camera2_obstacle_msg = ObstacleArrayMsg(
), ObstacleArrayMsg()
camera1_landmark_msg, camera2_landmark_msg = AprilTagDetectionArray(
), AprilTagDetectionArray()
camera1_marker_data, camera2_marker_data = MarkerArray(), MarkerArray()
camera1_obstacle_msg, camera1_marker_data = self.bbox_to_position_in_odom(
bboxes_from_camera1, Depth1image, camera1_param)
obstacle_msg, marker_data = self.bbox_to_position_in_odom(
bboxes_from_camera2, Depth2image, camera2_param,
len(camera1_obstacle_msg.obstacles), camera1_obstacle_msg,
camera1_marker_data)
def tag_callback(data, listener):
""" Callback when we receive new tag information """
tags = data
global curr_pose
# No detected tags
if len(tags.detections) == 0:
return
detections = tags.detections
min_dist = MAX_WORLD_SIZE
min_detection = AprilTagDetection()
# Use minimum distance to calculate position.
# The closer we are, the more accurate the estimate
for detection in detections:
dist = distance(detection.pose)
if dist < min_dist:
min_dist = dist
min_detection = detection
tag_name = 'tag_' + str(min_detection.id[0])
# Block if our transforms don't exist yet
listener.waitForTransform(tag_name, "camera", rospy.Time(0), rospy.Duration(3))
listener.waitForTransform("map", tag_name, rospy.Time(0), rospy.Duration(3))
# In the camera frame, camera is at (0,0,0)
camera_pose = PoseStamped()
camera_pose.header.frame_id = "camera"
camera_pose.pose.position.x = 0
camera_pose.pose.position.y = 0
camera_pose.pose.position.z = 0
p_in_tag = listener.transformPose(tag_name, camera_pose)
p_in_map = listener.transformPose("map", p_in_tag)
curr_pose.pose.position = p_in_map.pose.position
curr_pose.header.frame_id = "map"
curr_pose.header.stamp = rospy.Time.now()
def xbee_callback(msg_pre):
msg = msg_pre.data
case = msg[0]
# case 0 => Testing case
# case 1 => Apriltags Message Receive
if case == "0":
rospy.loginfo("This is a test message.")
rospy.loginfo((msg[1:]))
elif case == "1":
rospy.loginfo("This is an Apriltags message.")
at_array = AprilTagDetectionArray()
secs = int(msg[1:10])
nsecs = int(msg[10:19])
at_array.header.stamp.secs = secs
at_array.header.stamp.nsecs = nsecs
time_offset = 18
# Offset of each parameter
id_offset = 0
id_len = 3
# px
px_left_offset = id_offset + id_len
px_left_len = 2
px_right_offset = px_left_offset + px_left_len
px_right_len = 3
# py
py_left_offset = px_right_offset + px_right_len
py_left_len = 2
py_right_offset = py_left_offset + py_left_len
py_right_len = 3
# pz
pz_left_offset = py_right_offset + py_right_len
pz_left_len = 2
pz_right_offset = pz_left_offset + pz_left_len
pz_right_len = 3
# ox
ox_left_offset = pz_right_offset + pz_right_len
ox_left_len = 1
ox_right_offset = ox_left_offset + ox_left_len
ox_right_len = 12
# oy
oy_left_offset = ox_right_offset + ox_right_len
oy_left_len = 1
oy_right_offset = oy_left_offset + oy_left_len
oy_right_len = 12
# oz
oz_left_offset = oy_right_offset + oy_right_len
oz_left_len = 1
oz_right_offset = oz_left_offset + oz_left_len
oz_right_len = 12
# ow
ow_left_offset = oz_right_offset + oz_right_len
ow_left_len = 1
ow_right_offset = ow_left_offset + ow_left_len
ow_right_len = 12
# Watchtower id
wat_id_offset = ow_right_offset + ow_right_len
AT_OFFSET = ow_right_offset + ow_right_len
position_offset = 50 # Assume the range of pose -49~49
orientation_offset = 1 # Quaternion has range -1~1
print "Len: ", len(msg[1 + time_offset:])
print "Offset: ", AT_OFFSET
for i in range(len(msg[1 + time_offset:]) / AT_OFFSET):
detection = AprilTagDetection()
base_offset = i + 1 + time_offset
detection.id.append(int(msg[base_offset:base_offset + id_len]))
detection.pose.pose.pose.position.x = int(
msg[base_offset + px_left_offset:base_offset + px_left_offset +
px_left_len]) + float(
msg[base_offset + px_right_offset:base_offset +
px_right_offset +
px_right_len]) / 1000 - position_offset
detection.pose.pose.pose.position.y = int(
msg[base_offset + py_left_offset:base_offset + py_left_offset +
py_left_len]) + float(
msg[base_offset + py_right_offset:base_offset +
py_right_offset +
py_right_len]) / 1000 - position_offset
detection.pose.pose.pose.position.z = int(
msg[base_offset + pz_left_offset:base_offset + pz_left_offset +
pz_left_len]) + float(
msg[base_offset + pz_right_offset:base_offset +
pz_right_offset +
pz_right_len]) / 1000 - position_offset
detection.pose.pose.pose.orientation.x = int(
msg[base_offset + ox_left_offset:base_offset + ox_left_offset +
ox_left_len]) + float(
msg[base_offset + ox_right_offset:base_offset +
ox_right_offset +
ox_right_len]) / 1000000000000 - orientation_offset
detection.pose.pose.pose.orientation.y = int(
msg[base_offset + oy_left_offset:base_offset + oy_left_offset +
oy_left_len]) + float(
msg[base_offset + oy_right_offset:base_offset +
oy_right_offset +
oy_right_len]) / 1000000000000 - orientation_offset
detection.pose.pose.pose.orientation.z = int(
msg[base_offset + oz_left_offset:base_offset + oz_left_offset +
oz_left_len]) + float(
msg[base_offset + oz_right_offset:base_offset +
oz_right_offset +
oz_right_len]) / 1000000000000 - orientation_offset
detection.pose.pose.pose.orientation.w = int(
msg[base_offset + ow_left_offset:base_offset + ow_left_offset +
ow_left_len]) + float(
msg[base_offset + ow_right_offset:base_offset +
ow_right_offset +
ow_right_len]) / 1000000000000 - orientation_offset
detection.pose.header.frame_id = msg[base_offset + wat_id_offset:]
print "The Watchtower is: ", detection.pose.header.frame_id
at_array.detections.append(detection)
pub_apriltags.publish(at_array)
def data_cb(self, joint_msg, detection_msgs):
# set info-mat param
# reorder joint information
try:
jorder = ['waist', 'shoulder', 'elbow', 'hand', 'wrist']
j_idx = [joint_msg.name.index(j) for j in jorder]
j = [joint_msg.position[i] for i in j_idx]
#j = np.random.normal(loc=j, scale=0.005)
j = np.concatenate((j, [0])) # assume final joint is fixed
except Exception as e:
rospy.logerr_throttle(
1.0, 'Joint Positions Failed : {} \n {}'.format(e, joint_msg))
if len(detection_msgs.detections) <= 0:
# nothing detected, stop
return
# form pose ...
p, q = fk(self._dh0, j)
# testing ; add motion noise
# p = np.random.normal(p, scale=0.01)
# dq = qmath.rq(s=0.01)
# q = qmath.qmul(dq, q)
if not self._initialized:
# save previous pose and don't proceed
self._p = p
self._q = q
#self._slam.initialize(np.concatenate([p,q], axis=0))
self._initialized = True
return
# form observations ...
# nodes organized as:
# [base_link, landmarks{0..m-1}, poses{0..p-1}]
zs = []
try:
for pm in detection_msgs.detections:
m_id = pm.id[0]
ps = PoseStamped(header=pm.pose.header, pose=pm.pose.pose.pose)
# TODO : compute+apply static transform?
ps = self._tfl.transformPose('stereo_optical_link', ps)
zp, zq = pmsg2pq(ps.pose)
# testing; add observation noise
# zp = np.random.normal(zp, scale=0.05)
# dzq = qmath.rq(s=0.05)
# zq = qmath.qmul(dzq, zq)
dz = np.concatenate([zp, zq], axis=0)
zi = self._m2i[m_id]
if zi >= self._num_markers:
continue
self._i2m[zi] = m_id
zs.append([zi, dz, self._omega.copy()])
except Exception as e:
rospy.logerr_throttle(1.0, 'TF Failed : {}'.format(e))
return
### semi-offline batch optimization
ox = 1.0 * self._omega
self._graph.append([p, q, zs])
if len(self._graph) >= self._batch_size:
n_poses = 1 + len(self._graph) # add 1 for 0-node constraint
nodes = []
edges = []
p_nodes = []
z_nodes = [[] for _ in range(self._num_markers)]
# add 0-node
nodes.append(np.asarray([0, 0, 0, 0, 0, 0, 1]))
for gi, g in enumerate(self._graph):
xi = 1 + self._num_markers + gi # current pose index
# pose info
gp, gq, gz = g
gx = np.concatenate([gp, gq], axis=0)
p_nodes.append(gx)
edges.append([0, xi, gx, ox])
# TODO : handle scenarios when a landmark didn't appear in 200 observations
for zi, dz, zo in gz:
# edge from motion index to re-computed landmark index
edges.append([xi, 1 + zi, dz, zo])
zp_a, zq_a = qmath.x2pq(qmath.xadd_rel(gx, dz, T=False))
z_nodes[zi].append([zp_a, zq_a])
# add landmark nodes
if self._zinit:
# use the previous initialization for landmarks
for zp, zq in self._z_nodes:
zx = np.concatenate([zp, zq], axis=0)
nodes.append(zx)
else:
#initialize landmarks with average over 200 samples
for zi, zn in enumerate(z_nodes):
zp, zq = zip(*zn)
zp = np.mean(zp, axis=0)
zq = qmath.qmean(zq)
zx = np.concatenate([zp, zq], axis=0)
nodes.append(zx)
lms = nodes[1:1 + self._num_markers]
lms = [qmath.x2pq(x) for x in lms]
lms_re = [lms[self._m2i[i]] for i in range(self._num_markers)]
self._pub_ze.publish(msgn(lms_re, rospy.Time.now()))
self._zinit = True
# add motion nodes
for zx in p_nodes:
nodes.append(zx)
#print "PRE:"
#print nodes[-self._num_markers:]
nodes = self._slam.optimize(nodes,
edges,
max_iter=100,
tol=1e-12,
min_iter=100)
#print "POST:"
#print nodes[-self._num_markers:]
lms = nodes[1:1 + self._num_markers] # landmarks
lms = [qmath.x2pq(x) for x in lms]
self._z_nodes = lms
lms_re = [lms[self._m2i[i]] for i in range(self._num_markers)]
self._pub_ze.publish(msgn(lms_re, rospy.Time.now()))
self._graph.clear()
stamp = rospy.Time.now()
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = stamp
g_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
msg = AprilTagDetection()
m_id = self._i2m[i]
msg.id = [m_id]
msg.size = [0.0] # not really a thing
msg.pose.pose.pose = pose(lms[i][0], lms[i][1])
msg.pose.header.frame_id = 'base_link'
msg.pose.header.stamp = stamp
g_msg.detections.append(msg)
self._gt_pub.publish(g_msg)
return
### END: semi-offline batch optimization
### online slam with "reasonable" initialization?
#if not self._zinit:
# # add entry ...
# gx = np.concatenate([p,q], axis=0)
# for z in zs:
# _, zi, dz, _ = z
# zi = zi - 2
# zp_a, zq_a = qmath.x2pq(qmath.xadd_rel(gx, dz, T=False))
# self._z_nodes[zi].append( (zp_a, zq_a) )
# # check entries ...
# ls = [len(e) for e in self._z_nodes]
# if np.all(np.greater_equal(ls, 100)):
# znodes = []
# for zi, zn in enumerate(self._z_nodes):
# zp, zq = zip(*zn)
# zp = np.mean(zp, axis=0)
# zq = qmath.qmean(zq)
# zx = np.concatenate([zp,zq], axis=0)
# znodes.append(zx)
# self._slam._nodes[2+zi] = zx
# self._slam.initialize(gx)
# self._p = p
# self._q = q
# self._zinit = True
# return
### END : online slam with reasonable initialization
#dp, dq = qmath.xrel(self._p, self._q, p, q)
#dx = np.concatenate([dp,dq], axis=0)
dx = np.concatenate([p, q], axis=0)
self._slam._nodes[0] = np.asarray([0, 0, 0, 0, 0, 0, 1],
dtype=np.float32)
# save p-q
#self._p = p
#self._q = q
# for zp, zq in self._z_nodes:
# zx = np.concatenate([zp,zq], axis=0)
# nodes.append(zx)
# form dz ...
mu = self._slam.step(x=dx, zs=zs)
mu = np.reshape(mu, [-1, 7])
ep, eq = qmath.x2pq(mu[1])
gtp, gtq = fk(self._dh, j)
#perr = np.subtract(ep, gtp)
#perr = np.linalg.norm(perr)
#qerr = qmath.T(qmath.qmul(qmath.qinv(gtq), eq))
#qerr = ((qerr + np.pi) % (2*np.pi)) - np.pi
#qerr = np.linalg.norm(qerr)
#epe, eqe = perr, qerr
#perr = np.subtract(p, gtp)
#perr = np.linalg.norm(perr)
#qerr = qmath.T(qmath.qmul(qmath.qinv(gtq), q))
#qerr = ((qerr + np.pi) % (2*np.pi)) - np.pi
#qerr = np.linalg.norm(qerr)
#pe, qe = perr, qerr
#print pe-epe, qe-eqe
ez = [qmath.x2pq(e) for e in mu[2:]]
stamp = rospy.Time.now()
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = stamp
g_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
msg = AprilTagDetection()
m_id = self._i2m[i]
msg.id = [m_id]
msg.size = [0.0] # not really a thing
msg.pose.pose.pose = pose(ez[i][0], ez[i][1])
msg.pose.header.frame_id = 'base_link'
msg.pose.header.stamp = stamp
g_msg.detections.append(msg)
self._gt_pub.publish(g_msg)
#ez = [unparametrize(e) for e in mu[1:]]
self._p, self._q = ep.copy(), eq.copy()
#self._p, self._q = p, q
t = joint_msg.header.stamp
self._pub_ee.publish(msg1(ep, eq, t))
self._pub_ze.publish(msgn(ez, t))
def data_cb(self, joint_msg, detection_msgs):
try:
jorder = ['waist', 'shoulder', 'elbow', 'hand', 'wrist']
j_idx = [joint_msg.name.index(j) for j in jorder]
j = [joint_msg.position[i] for i in j_idx]
j = np.concatenate((j, [0])) # assume final joint is fixed
except Exception as e:
rospy.logerr_throttle(
1.0, 'Joint Positions Failed : {} \n {}'.format(e, joint_msg))
Xs = [np.eye(4) for _ in range(self._num_markers)]
vis = [False for _ in range(self._num_markers)]
if len(detection_msgs.detections) <= 0:
return
try:
for pm in detection_msgs.detections:
# pm.pose = pwcs
# pm.pose.pose = pwc
# pm.pose.pose.pose = p
# TODO : technically requires tf.transformPose(...) for robustness.
#pose = tf.transformPose(
ps = PoseStamped(header=pm.pose.header, pose=pm.pose.pose.pose)
# this transform should theoretically be painless + static
# Optionally, store the transform beforehand and apply it
ps = self._tfl.transformPose('stereo_optical_link', ps)
q = ps.pose.orientation
p = ps.pose.position
m_id = pm.id[0]
X = tx.compose_matrix(angles=tx.euler_from_quaternion(
[q.x, q.y, q.z, q.w]),
translate=[p.x, p.y, p.z])
i = self._m2i[m_id] # transform tag id to index
if i >= self._num_markers:
continue
self._i2m[i] = m_id
Xs[i] = X
vis[i] = True
self._seen[i] = True
except Exception as e:
rospy.logerr_throttle(1.0, 'TF Failed : {}'.format(e))
Xs = np.float32(Xs)
self._data.append((j, Xs, vis))
self._step += 1
rospy.loginfo_throttle(
1.0, 'Current Step : {}; vis : {}'.format(self._step, vis))
T = self._calib.eval_1(j, Xs, vis) # == (1, M, 4, 4)
now = rospy.Time.now()
m_msg = AprilTagDetectionArray()
m_msg.header.frame_id = 'base_link'
m_msg.header.stamp = now
pv_msg = PoseArray()
pv_msg.header.stamp = now
pv_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
if vis[i]:
txn = tx.translation_from_matrix(T[i])
rxn = tx.quaternion_from_matrix(T[i])
msg = AprilTagDetection()
msg.id = [self._i2m[i]]
msg.size = [0.0] # not really a thing
pwcs = msg.pose
pwcs.header.frame_id = 'base_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
m_msg.detections.append(msg)
pv_msg.poses.append(pwcs.pose.pose)
self._pub.publish(m_msg)
self._vpub.publish(pv_msg)
def publish(self):
now = rospy.Time.now()
if self._smooth:
# smooth change over time ...
if self._j_0 is None:
self._j_0 = np.random.uniform(low=-np.pi, high=np.pi, size=5)
self._j_1 = np.random.uniform(low=-np.pi, high=np.pi, size=5)
jpos = np.copy(self._j_0)
else:
self._m_i += 1
jpos = alerp(self._j_0, self._j_1, float(self._m_i) / self._m_n)
if self._m_i >= self._m_n:
self._j_0 = np.copy(self._j_1)
self._j_1 = np.random.uniform(low=-np.pi, high=np.pi, size=5)
self._m_i = 0
else:
# completely random
jpos = np.random.uniform(low=-np.pi, high=np.pi, size=5)
if self._zero:
jpos *= 0
txn, rxn = fk(self._dh, np.concatenate( [jpos, [0]] ))
## base_link --> object
M07 = [tx.compose_matrix(translate = xyz, angles=rpy) for (xyz, rpy) in zip(self._xyz, self._rpy)]
## base_link --> stereo_optical_link
M06 = tx.compose_matrix(
angles = tx.euler_from_quaternion(rxn),
translate = txn) # T_0_6
M06R = tx.quaternion_matrix(rxn)
Rt = M06R[:3,:3].T
T = -np.matmul(Rt, np.reshape(txn, [3,1]))
M06i = np.zeros((4,4), dtype=np.float32)
M06i[:3,:3] = Rt
M06i[:3, 3] = T[:,0]
M06i[ 3, 3] = 1.0
m_msg = AprilTagDetectionArray()
m_msg.header.stamp = now
m_msg.header.frame_id = 'stereo_optical_link'
pv_msg = PoseArray()
pv_msg.header.stamp = now
pv_msg.header.frame_id = 'stereo_optical_link'
M67 = [np.matmul(M06i, M) for M in M07]
for i in range(self._num_markers):
if np.random.random() > self._p:
continue
M = M67[i]
txn = tx.translation_from_matrix(M)
rxn = tx.quaternion_from_matrix(M)
if self._noise > 0.0:
txn = np.random.normal(loc=txn, scale=self._noise)
h = np.random.uniform(-self._noise, self._noise)
ax = np.random.normal(size=3)
ax /= np.linalg.norm(ax)
d_rxn = tx.quaternion_about_axis(h, ax)
rxn = tx.quaternion_multiply(d_rxn, rxn)
msg = AprilTagDetection()
msg.id = [i]
msg.size = [0.0] # not really a thing
pwcs = msg.pose
pwcs.header.frame_id = 'stereo_optical_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
m_msg.detections.append(msg)
pv_msg.poses.append(pwcs.pose.pose)
self._ppub.publish(m_msg)
self._pvpub.publish(pv_msg)
g_msg = AprilTagDetectionArray()
g_msg.header.stamp = now
g_msg.header.frame_id = 'base_link'
gv_msg = PoseArray()
gv_msg.header.stamp = now
gv_msg.header.frame_id = 'base_link'
for i in range(self._num_markers):
M = M07[i]
txn = tx.translation_from_matrix(M)
rxn = tx.quaternion_from_matrix(M)
msg = AprilTagDetection()
msg.id = [i]
msg.size = [0.0] # not really a thing
pwcs = msg.pose
pwcs.header.frame_id = 'base_link'
pwcs.header.stamp = now
fill_pose_msg(pwcs.pose.pose, txn, rxn)
g_msg.detections.append(msg)
gv_msg.poses.append(pwcs.pose.pose)
if self._ground_truth:
self._gpub.publish(g_msg)
# visualizations are always published
self._gvpub.publish(gv_msg)
#gmsg = PoseStamped()
#gmsg.header.frame_id = 'base_link'
#txn = tx.translation_from_matrix(M07)
#rxn = tx.quaternion_from_matrix(M07)
#gmsg.header.stamp = now
#fill_pose_msg(gmsg.pose, txn, rxn)
#self._gpub.publish(gmsg)
jmsg = JointState()
jmsg.header.stamp = now
jmsg.header.frame_id = 'map' #??
jmsg.name = ['waist', 'shoulder', 'elbow', 'hand', 'wrist']
jmsg.position = jpos
jmsg.velocity = np.zeros_like(jpos)
jmsg.effort = np.zeros_like(jpos)
self._jpub.publish(jmsg)