def callback(self, target_oe, chaser_oe):
# calculate baseline
osc_O_t = rospace_lib.KepOrbElem()
osc_O_c = rospace_lib.KepOrbElem()
osc_O_t.from_message(target_oe.position)
osc_O_c.from_message(chaser_oe.position)
# convert to TEME
S_t = rospace_lib.CartesianTEME()
S_c = rospace_lib.CartesianTEME()
S_t.from_keporb(osc_O_t)
S_c.from_keporb(osc_O_c)
# vector from chaser to target in chaser body frame in [m]
# get current rotation of body
R_J2K_CB = transformations.quaternion_matrix([
chaser_oe.orientation.x, chaser_oe.orientation.y,
chaser_oe.orientation.z, chaser_oe.orientation.w
])
r_J2K = (S_t.R - S_c.R) * 1000
r_CB = np.dot(R_J2K_CB[0:3, 0:3].T, r_J2K)
# publish observation as marker for visualization
msg = Marker()
msg.header.frame_id = "chaser_1"
msg.type = Marker.ARROW
msg.action = Marker.ADD
msg.points.append(Point(0, 0, 0))
msg.points.append(Point(r_CB[0], r_CB[1], r_CB[2]))
msg.scale.x = 100
msg.scale.y = 200
msg.color.a = 1.0
msg.color.r = 0.9
msg.color.g = 0.5
msg.color.b = 0.1
self.pub_m.publish(msg)
# check if visible and augment sensor value
visible, value = sensor_obj.get_measurement(r_CB)
msg = AzimutElevationRangeStamped()
msg.header.stamp = target_oe.header.stamp
# throttle publishing to maximum rate
if not (target_oe.header.stamp -
self.last_publish_time).to_sec() > 1.0 / self.rate:
return
# if target is visible, publish message
if visible:
# these measurements already have noise added
msg.value.azimut = value[0]
msg.value.elevation = value[1]
msg.valid = True
# if range measurement is activated
if len(sensor_obj.mu) == 3:
msg.value.range = np.linalg.norm(r_CB) + np.asscalar(
np.random.normal(sensor_obj.mu[2], sensor_obj.sigma[2], 1))
else:
msg.valid = False
self.pub.publish(msg)
self.last_publish_time = target_oe.header.stamp
