def pose_callback(self, msg):
'''handles arrival of pose messages
:type msg: PoseStamped'''
cam_pose = msg.pose
# two camera poses and TFs are available
if self.last_cam_pose is not None and self.has_tfs:
# actually not the base_link poses, but camera poses from perspective of base_link frame
last_base_pose = tf2_geometry_msgs.do_transform_pose(
PoseStamped(pose=self.last_cam_pose), self.base_link_to_camera_tf).pose
base_pose = tf2_geometry_msgs.do_transform_pose(
PoseStamped(pose=cam_pose), self.base_link_to_camera_tf).pose
# transform to actual base_link poses (still in base_link frame)
transform_pose(last_base_pose, self.camera_to_base_link_tf.transform)
transform_pose(base_pose, self.camera_to_base_link_tf.transform)
# get difference
delta_transform = get_relative_transform(
last_base_pose.position, last_base_pose.orientation, base_pose.position, base_pose.orientation)
# everything seems to be working fine
if not self.enabled:
rospy.loginfo('ORBSLAM2PoseSubscriber enabled')
self.enabled = True
# hand in the update
self.callback(delta_transform)
if self.do_plot:
plotting.add_transform(self, delta_transform)
# delay poses until TFs initialized
if self.last_cam_pose is None or self.has_tfs:
self.last_cam_pose = cam_pose
def pose_callback(self, msg):
'''handles arrival of pose messages'''
# strip potential header and cov
pose, stamp = (msg.pose, msg.header.stamp) if self.is_stamped else (msg, None)
pose = pose.pose if self.is_with_covariance else pose
# IMU enhanced mode
if self.is_roll_pitch_imu:
# check IMU availability
imu = self.imu_subscriber.get_last_imu() if stamp is None else self.imu_subscriber.get_imu_at_time(stamp)
if imu is None:
rospy.logwarn('PoseSubscriber: IMU data unavailable. Suspending pose updates.')
self.enabled = False
return
self.global_orientation = imu.orientation
# replace roll and pitch with IMU values
_, pitch, roll = euler_from_quaternion(unpack_quaternion(imu.orientation), axes='rzyx')
yaw, _, _ = euler_from_quaternion(unpack_quaternion(pose.orientation), axes='rzyx')
pose.orientation = Quaternion(*quaternion_from_euler(yaw, pitch, roll, axes='rzyx'))
if self.last_pose is not None:
# get difference
delta_transform = get_relative_transform(
self.last_pose.position, self.last_pose.orientation,
pose.position, pose.orientation)
self.enabled = True
# hand in the update
self.callback(delta_transform)
if self.do_plot:
plotting.add_transform(self, delta_transform)
self.last_pose = pose
self.last_pose_time = stamp
def __init__(self,
gps_topic='/mavros/global_position/global',
initial_heading=0.,
**kwargs):
super(GPSSubscriber, self).__init__(**kwargs)
# params
self.gps_topic = gps_topic
self.orig_ll = None
self.last_position = Vector3()
if self.do_plot and initial_heading != 0.:
q = Quaternion(*quaternion_from_euler(0, 0, initial_heading))
plotting.add_transform(
self, Transform(translation=Vector3(0., 0., 0.), rotation=q))
self.sub_gps = rospy.Subscriber(self.gps_topic, NavSatFix,
self.gps_callback)
def state_callback(self, msg):
'''handles arrival of ORBState messages, validates queued TFs and processes them'''
if msg.state == ORBState.OK:
# update 'ok' time frame
if self.first_ok_time is None:
self.first_ok_time = msg.header.stamp
self.last_ok_time = msg.header.stamp
while len(self.tf_queue) > 1: # need two or moar for a diff
# discard an invalid TF
if self.tf_queue[0].header.stamp < self.first_ok_time:
self.tf_queue.popleft()
continue
# pending 'ok' state
if self.tf_queue[0].header.stamp > self.last_ok_time or self.tf_queue[1].header.stamp > self.last_ok_time:
# wait for clearance
return
# assume sequential timestamps
assert(self.tf_queue[0].header.stamp <= self.tf_queue[1].header.stamp)
# everything seems to be working fine
if not self.enabled:
rospy.loginfo('ORBSLAM2Subscriber enabled')
self.enabled = True
# get TFs and let's go
tf1 = self.tf_queue.popleft().transform
tf2 = self.tf_queue[0].transform
# get difference
delta_transform = get_relative_transform(tf1.translation, tf1.rotation, tf2.translation, tf2.rotation)
# hand in the update
self.callback(delta_transform)
if self.do_plot:
plotting.add_transform(self, delta_transform)
rospy.logdebug('ORBSLAM2Subscriber: tf processed')
else:
# not 'ok'
self.disable()
def imu_callback(self, msg):
'''handles arrival of Imu messages
:type msg: Imu'''
# maintain IMU log for `get_imu_at_time`
if self.is_imu_log_enabled:
with self.imu_queue_mutex:
self.imu_queue.append(msg)
# discard old messages
while self.imu_queue[
0].header.stamp + self.imu_log_duration < msg.header.stamp:
self.imu_queue.popleft()
# provide rotation
if self.callback is not None and self.last_imu is not None:
# check if `orientation` is available (it's a bit smoother than `angular_velocity`)
if self.last_imu.orientation_covariance[
0] != -1 and msg.orientation_covariance[0] != -1:
delta_transform = get_relative_transform(
Vector3(), self.last_imu.orientation, Vector3(),
msg.orientation)
else:
# integrate angular velocities instead
dt = (msg.header.stamp - self.last_imu.header.stamp).to_sec()
translation = Vector3(
) # TODO: consider integrating linear acceleration
av = msg.angular_velocity
rotation = quaternion_from_euler(dt * av.x, dt * av.y,
dt * av.z)
delta_transform = Transform(translation=translation,
rotation=get_quaternion(rotation))
# hand in the update
self.callback(delta_transform)
if self.do_plot:
plotting.add_transform(self, delta_transform)
self.last_imu = msg
def gps_callback(self, msg):
'''handles arrival of NavSatFix messages'''
if self.orig_ll is None:
self.orig_ll = msg.latitude, msg.longitude
# position difference
x, y = latlon_to_dxdy(msg.latitude, msg.longitude, *self.orig_ll)
z = msg.altitude
position = Vector3(x, y, z)
translation = get_vector3_subtraction(position, self.last_position)
# construct a transform
delta_transform = Transform(
translation=translation,
rotation=Quaternion(
0, 0, 0, 1
) # The commonly-used unit quaternion that yields no rotation about the x/y/z axes is (0,0,0,1) [http://wiki.ros.org/tf2/Tutorials/Quaternions]
)
# hand in the update
self.callback(delta_transform)
if self.do_plot:
plotting.add_transform(self, delta_transform)
self.last_position = position