Help wanted: set timestamp when feeding MOCAP data into /VehicleVisualOdometry

[ErcBunny]

Hello, I want to relay the geometry_msgs/msg/PoseStamped type of data from the motion capture system to px4_msgs/msg/VehicleVisualOdometry via micrortps_bridge, but I have no idea how to set timestamp and timestamp_sample.

I guess I should use the timestamp in px4_msgs/msg/Timesync_PubSubTopic instead of the one in geometry_msgs/msg/PoseStamped, right? But I notice that the update rate of px4_msgs/msg/Timesync_PubSubTopic is only 20Hz and my pose data comes in at a higher rate (50Hz). I wonder if it is OK to use the Timesync topic, or if it is better to use timestamp data that comes with VehicleLocalPosition because it is updated at 100Hz.

So what is the right way to fill the timestamp data? What is the mechanism behind "time syncing"? Is there any documentation concerning this issue? Thank you for reading, regards~

[TheLonelyFighter]

You can do it like this: just use Timesync_PubSubTopic to get the correct timestamp

class MinimalSubscriber(Node):

def __init__(self):
    super().__init__('minimal_subscriber')
    self.subscription = self.create_subscription(
        PoseStamped,
        '/vrpn_client_node/marius_drone/pose',
        self.optitrack_callback,
        10)
    self.subscription  # prevent unused variable warning

    self.current_timestamp = 0
    self.last_timestamp = 0

    self.timesync_sub = self.create_subscription(Timesync, 'Timesync_PubSubTopic', self.timesync_callback, 10)
    self.visual_odom_pub = self.create_publisher(VehicleVisualOdometry, '/VehicleVisualOdometry_PubSubTopic', 10)

def timesync_callback(self, msg):
    self.last_timestamp = self.current_timestamp
    self.current_timestamp = msg.timestamp

def optitrack_callback(self, msg):

    visual_odom_msg = VehicleVisualOdometry()
    
    visual_odom_msg.x = msg.pose.position.x
    visual_odom_msg.y = msg.pose.position.y
    visual_odom_msg.z = msg.pose.position.z
    visual_odom_msg.timestamp = self.current_timestamp 
    visual_odom_msg.timestamp_sample = self.last_timestamp
    visual_odom_msg.q[0] = msg.pose.orientation.x
    visual_odom_msg.q[1] = msg.pose.orientation.y
    visual_odom_msg.q[2] = msg.pose.orientation.z
    visual_odom_msg.q[3] = msg.pose.orientation.w

    self.visual_odom_pub.publish(visual_odom_msg)     

[ErcBunny]

Hi @TheLonelyFighter,

Thanks for commenting. I want to add that using the timestamp data that comes with VehicleLocalPosition (100Hz) is also fine (verified on real hardware).

Cheers~