mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
902 stars 993 forks source link

vision_position_estimate time stamp use only the nanosecond part #1858

Open robobe opened 1 year ago

robobe commented 1 year ago

Issue details

I'm use mavros with ArduPilot and try to develop system without GPS dependency I have visual system that can tell me the distance from EKF Origin I'm try to use the VISION_POSITION_ESTIMATE (102) message by using vision_pose_estimate plugin I have issue with the time stamp field that use only the nanoseconds part The mavlink and the ardupilot code use EPOC time or boot time in micro seconds unit

mavlink::common::msg::VISION_POSITION_ESTIMATE vp{};

    vp.usec = stamp.nanoseconds() / 1000;
    vp.x = position.x();
    vp.y = position.y();
    vp.z = position.z();
    vp.roll = rpy.x();
    vp.pitch = rpy.y();
    vp.yaw = rpy.z();

Why not calc the mavlink usec field from the whole stamp message ?

MAVROS version and platform

Mavros: 2.4.0 ROS: HUMBLE Ubuntu: 22.04

Autopilot type and version

[ X] ArduPilot [ ] PX4

Version: 4.5

vooon commented 1 year ago

Because it's the only nanoseconds field used:

https://github.com/ros2/rclcpp/blob/b812790ee301d9aa536c79b9636736471701d1c3/rclcpp/src/rclcpp/time.cpp#L224-L228