CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_DDS/AP_DDS_External_Odom.cpp
Views: 1798
1


2


3
#include "AP_DDS_External_Odom.h"

4
#include "AP_DDS_Type_Conversions.h"

5


6
#if AP_DDS_VISUALODOM_ENABLED

7


8
#include <AP_VisualOdom/AP_VisualOdom.h>

9
#include <GCS_MAVLink/GCS.h>

10


11
void AP_DDS_External_Odom::handle_external_odom(const tf2_msgs_msg_TFMessage& msg)

12
{

13
    auto *visual_odom = AP::visualodom();

14
    if (visual_odom == nullptr) {

15
        return;

16
    }

17


18
    for (size_t i = 0; i < msg.transforms_size; i++) {

19
        const auto& ros_transform_stamped = msg.transforms[i];

20
        if (!is_odometry_frame(ros_transform_stamped)) {

21
            continue;

22
        }

23
        const uint64_t remote_time_us {AP_DDS_Type_Conversions::time_u64_micros(ros_transform_stamped.header.stamp)};

24


25
        Vector3f ap_position;

26
        Quaternion ap_rotation;

27


28
        convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation);

29
        // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed.

30
        // Before propagating a potentially inaccurate quaternion to the rest of AP, normalize it here.

31
        // TODO what if the quaternion is NaN?

32
        ap_rotation.normalize();

33


34
        // No error is available in TF, trust the data as-is

35
        const float posErr {0.0};

36
        const float angErr {0.0};

37
        // The odom to base_link transform used is locally consistent per ROS REP-105.

38
        // https://www.ros.org/reps/rep-0105.html#id16

39
        // Thus, there will not be any resets.

40
        const uint8_t reset_counter {0};

41
        // TODO implement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg));

42
        const uint32_t time_ms {static_cast<uint32_t>(remote_time_us * 1E-3)};

43
        visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter, 0);

44


45
    }

46
}

47


48
bool AP_DDS_External_Odom::is_odometry_frame(const geometry_msgs_msg_TransformStamped& msg)

49
{

50
    char odom_parent[] = "odom";

51
    char odom_child[] = "base_link";

52
    // Assume the frame ID's are null terminated.

53
    return (strcmp(msg.header.frame_id, odom_parent) == 0) &&

54
           (strcmp(msg.child_frame_id, odom_child) == 0);

55
}

56


57
void AP_DDS_External_Odom::convert_transform(const geometry_msgs_msg_Transform& ros_transform, Vector3f& translation, Quaternion& rotation)

58
{

59
    // convert from x-forward, y-left, z-up to NED

60
    // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130

61
    translation = {

62
        static_cast<float>(ros_transform.translation.x),

63
        static_cast<float>(-ros_transform.translation.y),

64
        static_cast<float>(-ros_transform.translation.z)

65
    };

66


67
    // In AP, q1 is the quaternion's scalar component.

68
    // In ROS, w is the quaternion's scalar component.

69
    // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion

70
    rotation.q1 = ros_transform.rotation.w;

71
    rotation.q2 = ros_transform.rotation.x;

72
    rotation.q3 = -ros_transform.rotation.y;

73
    rotation.q4 = -ros_transform.rotation.z;

74
}

75


76
#endif // AP_DDS_VISUALODOM_ENABLED

77


78