| 762 | } |
| 763 | |
| 764 | void RosBaseDataProvider::publishStaticTf(const gtsam::Pose3& pose, |
| 765 | const std::string& parent_frame_id, |
| 766 | const std::string& child_frame_id) { |
| 767 | static tf2_ros::StaticTransformBroadcaster static_broadcaster; |
| 768 | geometry_msgs::TransformStamped static_transform_stamped; |
| 769 | // TODO(Toni): Warning: using ros::Time::now(), will that bring issues? |
| 770 | static_transform_stamped.header.stamp = ros::Time::now(); |
| 771 | static_transform_stamped.header.frame_id = parent_frame_id; |
| 772 | static_transform_stamped.child_frame_id = child_frame_id; |
| 773 | static_transform_stamped.transform.translation.x = pose.x(); |
| 774 | static_transform_stamped.transform.translation.y = pose.y(); |
| 775 | static_transform_stamped.transform.translation.z = pose.z(); |
| 776 | const gtsam::Quaternion& quat = pose.rotation().toQuaternion(); |
| 777 | static_transform_stamped.transform.rotation.x = quat.x(); |
| 778 | static_transform_stamped.transform.rotation.y = quat.y(); |
| 779 | static_transform_stamped.transform.rotation.z = quat.z(); |
| 780 | static_transform_stamped.transform.rotation.w = quat.w(); |
| 781 | static_broadcaster.sendTransform(static_transform_stamped); |
| 782 | } |
| 783 | |
| 784 | void RosBaseDataProvider::printParsedParams() const { |
| 785 | LOG(INFO) << std::string(80, '=') << '\n' |
nothing calls this directly
no outgoing calls
no test coverage detected