ROS Publishing On a Topic, But Not With the Right Values

edit

Hello! I'm new to ROS, so apologies if this is just due to me doing something dumb, but I can't figure out what's going on with my publisher. I wrote a simple node in C++ that reads geometry_msgs::PoseStamped messages from one topic, duplicates them, and just publishes them directly to another topic. At least, that's what it's supposed to do: my node successfully advertises on the new topic and publishes PoseStamped messages, but the messages it publishes are unrelated to the messages it reads in. The values in the PostStamped messages are different for each message, but after the first message (which I think matches the first message read by the node) they appear to stay within 0.1% of the values in the first message, regardless of how the messages read by the node vary over time. Here is my code:

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

geometry_msgs::PoseStamped current;
bool ready_to_transmit = false;
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
  current = *msg;
  ready_to_transmit = true;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "vicon_node");
  ros::NodeHandle nh;

  ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
    ("vrpn_client_node/quadpeter/pose", 1, pose_cb);
  ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>
    ("mavros/vision_pose/pose", 1);

  bool printed = false;

  while (ros::ok()) {
    ros::spinOnce();

    if (!ready_to_transmit) continue;

    if (!printed) {
      ROS_INFO("Ready to transmit position data");
      printed = true;
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = current.pose.position.x;
    pose.pose.position.y = current.pose.position.y;
    pose.pose.position.z = current.pose.position.z;
    pose.pose.orientation.x = current.pose.orientation.x;
    pose.pose.orientation.y = current.pose.orientation.y;
    pose.pose.orientation.z = current.pose.orientation.z;
    pose.pose.orientation.w = current.pose.orientation.w;

    pose_pub.publish(pose);
  }

  return 0;
}

I am using rostopic echo to check the messages read in and published out. EDIT: I am away from the Up board for a couple days and will post the terminal window contents when I get a chance, but in the meantime if anyone's seen this problem before or sees any issues with the code I'd really appreciate any tips. Thank you!

I am using Ubuntu 18.04 on an Up board (sort of like a suped-up Raspberry Pi) with kernel 4.15.0 for Up.

Probably not critical to my question, but for context, the ultimate goal for this node is to read data from a Vicon system, do simple transformations on it, and send it to PX4 via MAVROS for external position estimation so that I can control a quadrotor drone with motion capture. I am reading position data from the vrpn_client_ros package using its sample.launch file.