Refine Transforms

[akevinge]

Currently, transforms are broadcasting GNSS-based odometry. For various reasons, including noise and instability, we should look to combine GNSS and IMU (/ouster/imu) to broadcast proper transforms.

Current code:

vehicle_interface/src/sensing/gnss/gnss/gnss_interface_node.py

Lines 89 to 105 in 3c9cc17

	def publishTf(self):
	if self.cached_odom is None:
	return
	# Publish our map->base_link tf
	t = TransformStamped()
	t.header = self.cached_odom.header
	t.child_frame_id = self.cached_odom.child_frame_id
	transl = Vector3()
	transl.x = self.cached_odom.pose.pose.position.x
	transl.y = self.cached_odom.pose.pose.position.y
	transl.z = self.cached_odom.pose.pose.position.z
	t.transform.translation = transl
	t.transform.rotation = self.cached_odom.pose.pose.orientation
	# self.get_logger().error(str(t))
	self.tf_broadcaster.sendTransform(t)

Ideas:

• GTSAM Example of GPS/IMU
• EFK or UFK on top of IMU data