Switch from vio_pos_vel to pos_vel broke transformation publishing #2
Description
Commit 0fc2789 introduced the following change:
-void SnavInterface::UpdateVIOMessages(tf2::Quaternion q)
+void SnavInterface::UpdatePosVelMessages(tf2::Quaternion q)
{
- // -------------------------------
- // -- VIO TFs and Pose Messages --
- // -------------------------------
- tf2::Transform vio_tf(tf2::Transform(q, tf2::Vector3(cached_data_->vio_pos_vel.position_estimated[0],
- cached_data_->vio_pos_vel.position_estimated[1],
- cached_data_->vio_pos_vel.position_estimated[2])));
-
- // The tf tree must maintain a strict tree structure. As such, base_link cannont have multiple parents.
- // If you want to use (for example) GPS and VIO at the same time, you must choose one of them to be the
- // parent of base_link. This is done throught the root_tf_frame param. The other potential base_link parents
- // (for example gps and optic flow) will have their transform inverted such that base_link is their parent
- if(!vio_is_root_tf_)
- {
- vio_tf = vio_tf.inverse();
- vio_transform_.child_frame_id = vio_frame_;
- vio_transform_.header.frame_id = base_link_frame_;
- }
- else
- {
- vio_transform_.child_frame_id = base_link_frame_;
- vio_transform_.header.frame_id = vio_frame_;
- }
+ tf2::Transform est_tf(tf2::Transform(q, tf2::Vector3(
+ cached_data_->pos_vel.position_estimated[0],
+ cached_data_->pos_vel.position_estimated[1],
+ cached_data_->pos_vel.position_estimated[2])));Now when running this node, the transformation information from the /pose topic is always (0, 0, 0). When I populate est_tf with vio_pos_vel.position_estimated instead of pos_vel.position_estimated, valid transformation data is output.
Why is pos_vel not being populated (and vio_pos_vel is)? Is there an initialization step I am missing?
Thanks.