| 616 | } |
| 617 | |
| 618 | void RosBaseDataProvider::publishTf(const SpinOutputPacket& vio_output) { |
| 619 | const Timestamp& timestamp = vio_output.getTimestamp(); |
| 620 | const gtsam::Pose3& pose = vio_output.getEstimatedPose(); |
| 621 | const gtsam::Quaternion& quaternion = pose.rotation().toQuaternion(); |
| 622 | // Publish base_link TF. |
| 623 | geometry_msgs::TransformStamped odom_tf; |
| 624 | odom_tf.header.stamp.fromNSec(timestamp); |
| 625 | odom_tf.header.frame_id = world_frame_id_; |
| 626 | odom_tf.child_frame_id = base_link_frame_id_; |
| 627 | |
| 628 | odom_tf.transform.translation.x = pose.x(); |
| 629 | odom_tf.transform.translation.y = pose.y(); |
| 630 | odom_tf.transform.translation.z = pose.z(); |
| 631 | odom_tf.transform.rotation.w = quaternion.w(); |
| 632 | odom_tf.transform.rotation.x = quaternion.x(); |
| 633 | odom_tf.transform.rotation.y = quaternion.y(); |
| 634 | odom_tf.transform.rotation.z = quaternion.z(); |
| 635 | tf_broadcaster_.sendTransform(odom_tf); |
| 636 | } |
| 637 | |
| 638 | void RosBaseDataProvider::publishFrontendStats( |
| 639 | const SpinOutputPacket& vio_output) const { |
nothing calls this directly
no outgoing calls
no test coverage detected