GDK TF Interface Documentation (C++)

Overview

The TF (coordinate transform) module provides G02 robots with a coordinate transform query capability. Through the C++ interface, developers can conveniently obtain the coordinate transform relationships between the robot's various components, suitable for coordinate transformation, sensor calibration, multi-sensor fusion, SLAM mapping, and many other scenarios.

Interface Description

TF Class

This class encapsulates the main query interfaces for coordinate transforms.

1. GetAllTfFromBaseLink()

• Function: Get the transform relationships from base_link to all child coordinate frames
• Parameters:

Parameter	Type	Description
transforms	std::vector<TransformStamped>&	Output parameter, list of transform relationships

• Return value: GDKRes, an operation result status code. Returns GDKRes::kSuccess on success, and the transforms parameter contains all transform relationships

TransformStamped Object — Detailed Description

The TransformStamped struct contains the following members:

Member	Type	Description	Unit
frame_id	std::string	Parent coordinate frame ID	String
child_frame_id	std::string	Child coordinate frame ID	String
transform	Transform	Transform information	Transform object
timestamp_ns	uint64_t	Timestamp	Nanoseconds

struct TransformStamped {
  std::string frame_id{};        ///< frame id
  std::string child_frame_id{};  ///< child frame id
  Transform transform{};         ///< transform
  uint64_t timestamp_ns{0};      ///< transform timestamp(ns)
};

Transform struct:

Member	Type	Description	Unit
translation	Vector3	Translation vector	Meters
rotation	Quaternion	Rotation quaternion	None

struct Transform {
  Vector3 translation{};  ///< translation
  Quaternion rotation{};  ///< rotation
};

Vector3 struct:

Member	Type	Description	Unit
x	double	X-axis component	Meters
y	double	Y-axis component	Meters
z	double	Z-axis component	Meters

Quaternion struct:

Member	Type	Description	Unit
x	double	Quaternion X component	None
y	double	Quaternion Y component	None
z	double	Quaternion Z component	None
w	double	Quaternion W component	None

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    std::vector<agibot::gdk::TransformStamped> transforms;
    if (tf.GetAllTfFromBaseLink(transforms) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get all transforms" << std::endl;
    } else {
        std::cout << "Got " << transforms.size() << " transform relationship(s):" << std::endl;
        for (const auto& transform_stamped : transforms) {
            std::cout << "Frame: " << transform_stamped.frame_id
                      << " -> " << transform_stamped.child_frame_id << std::endl;
            std::cout << "  Translation: x=" << transform_stamped.transform.translation.x
                      << ", y=" << transform_stamped.transform.translation.y
                      << ", z=" << transform_stamped.transform.translation.z << std::endl;
            std::cout << "  Rotation: x=" << transform_stamped.transform.rotation.x
                      << ", y=" << transform_stamped.transform.rotation.y
                      << ", z=" << transform_stamped.transform.rotation.z
                      << ", w=" << transform_stamped.transform.rotation.w << std::endl;
            std::cout << "  Timestamp: " << transform_stamped.timestamp_ns << std::endl;
            std::cout << std::endl;
        }
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

2. GetTfFromBaseLink()

• Function: Get the transform relationship from base_link to a specified child coordinate frame
• Parameters:

Parameter	Type	Description
child_frame_id	const std::string&	Child coordinate frame ID
transform	Transform&	Output parameter, transform information object

• Return value: GDKRes, an operation result status code. Returns GDKRes::kSuccess on success, and the transform parameter contains the transform information

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::Transform transform;
    std::string child_frame_id = "arm_l_end_link";

    if (tf.GetTfFromBaseLink(child_frame_id, transform) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get transform for " << child_frame_id << std::endl;
    } else {
        std::cout << "Transform from base_link to " << child_frame_id << ":" << std::endl;
        std::cout << "  Translation: x=" << transform.translation.x
                  << ", y=" << transform.translation.y
                  << ", z=" << transform.translation.z << std::endl;
        std::cout << "  Rotation: x=" << transform.rotation.x
                  << ", y=" << transform.rotation.y
                  << ", z=" << transform.rotation.z
                  << ", w=" << transform.rotation.w << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

3. GetTfFromSensor()

• Function: Get a sensor's extrinsic transform relationship
• Parameters:

Parameter	Type	Description
sensor_extrinsic_type	const SensorExtrinsicType&	Sensor extrinsic type enum
transform	Transform&	Output parameter, transform information object

• Return value: GDKRes, an operation result status code. Returns GDKRes::kSuccess on success, and the transform parameter contains the sensor's extrinsic transform information

SensorExtrinsicType Enum — Detailed Description

Supported sensor extrinsic types:

Enum value	Description
kUnknown	Unknown type
kHeadLeftStereoToHeadRightStereo	Head left stereo camera to right stereo camera
kLeftHandDepthToLeftHandColor	Left-hand depth camera to color camera
kRightHandDepthToRightHandColor	Right-hand depth camera to color camera
kHeadDepthToHeadColor	Head depth camera to color camera
kHeadLeftStereoToHeadLink3	Head left stereo camera to head link 3
kHeadRightStereoToHeadLink3	Head right stereo camera to head link 3
kHeadLeftFisheyeToHeadLink3	Head left fisheye camera to head link 3
kHeadRightFisheyeToHeadLink3	Head right fisheye camera to head link 3
kHeadBackFisheyeToHeadLink3	Head rear fisheye camera to head link 3
kChassisFrontLidarToBaseLink	Chassis front lidar to base_link
kChassisBackLidarToBaseLink	Chassis rear lidar to base_link
kChassisBackLidarToChassisFrontLidar	Chassis rear lidar to front lidar
kChassisMid360ImuToChassisMid360Lidar	Chassis Mid360 IMU to chassis Mid360 lidar
kChassisImuToBaseLink	Chassis IMU to base_link
kLeftHandRGBDToArmLEndLink	Left-hand RGBD to left arm end link
kRightHandRGBDToArmREndLink	Right-hand RGBD to right arm end link
kHeadRGBDToHeadLink3	Head RGBD to head link 3

enum class SensorExtrinsicType {
  kUnknown = 0,
  kHeadLeftStereoToHeadRightStereo,
  kLeftHandDepthToLeftHandColor,
  kRightHandDepthToRightHandColor,
  kHeadDepthToHeadColor,
  kHeadLeftStereoToHeadLink3,
  kHeadRightStereoToHeadLink3,
  kHeadLeftFisheyeToHeadLink3,
  kHeadRightFisheyeToHeadLink3,
  kHeadBackFisheyeToHeadLink3,
  kChassisFrontLidarToBaseLink,
  kChassisBackLidarToBaseLink,
  kChassisBackLidarToChassisFrontLidar,
  kChassisMid360ImuToChassisMid360Lidar,
  kChassisImuToBaseLink,
  kLeftHandRGBDToArmLEndLink,
  kRightHandRGBDToArmREndLink,
  kHeadRGBDToHeadLink3
};

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::Transform transform;
    agibot::gdk::SensorExtrinsicType sensor_type =
        agibot::gdk::SensorExtrinsicType::kHeadLeftStereoToHeadRightStereo;

    if (tf.GetTfFromSensor(sensor_type, transform) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get sensor extrinsic transform" << std::endl;
    } else {
        std::cout << "Sensor extrinsic transform:" << std::endl;
        std::cout << "  Translation: x=" << transform.translation.x
                  << ", y=" << transform.translation.y
                  << ", z=" << transform.translation.z << std::endl;
        std::cout << "  Rotation: x=" << transform.rotation.x
                  << ", y=" << transform.rotation.y
                  << ", z=" << transform.rotation.z
                  << ", w=" << transform.rotation.w << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

4. LookupTransformLatest()

• Function: Query the latest transform relationship between two coordinate frames
• Parameters:

Parameter	Type	Description
target_frame	const std::string&	Target coordinate frame ID
source_frame	const std::string&	Source coordinate frame ID
transform	Transform&	Output parameter, transform information object (from source to target)
timestamp_ns	uint64_t*	Output parameter, pointer to the timestamp (optional, may be nullptr)

• Return value: GDKRes, an operation result status code. Returns GDKRes::kSuccess on success, and the transform parameter contains the transform information

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::Transform transform;
    uint64_t timestamp_ns = 0;

    // Query the latest transform from arm_l_end_link to base_link
    if (tf.LookupTransformLatest("base_link", "arm_l_end_link", transform, &timestamp_ns)
        != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to lookup transform" << std::endl;
    } else {
        std::cout << "Latest transform from arm_l_end_link to base_link:" << std::endl;
        std::cout << "  Translation: x=" << transform.translation.x
                  << ", y=" << transform.translation.y
                  << ", z=" << transform.translation.z << std::endl;
        std::cout << "  Rotation: x=" << transform.rotation.x
                  << ", y=" << transform.rotation.y
                  << ", z=" << transform.rotation.z
                  << ", w=" << transform.rotation.w << std::endl;
        std::cout << "  Timestamp: " << timestamp_ns << " ns" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

5. LookupTransform()

• Function: Query the transform relationship between two coordinate frames at a specific time (supports time interpolation)
• Parameters:

Parameter	Type	Description
target_frame	const std::string&	Target coordinate frame ID
source_frame	const std::string&	Source coordinate frame ID
time_ns	uint64_t	Query time (nanosecond timestamp)
transform	Transform&	Output parameter, transform information object (from source to target)

• Return value: GDKRes, an operation result status code. Returns GDKRes::kSuccess on success, and the transform parameter contains the transform information

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Get the current timestamp
    auto now = std::chrono::system_clock::now();
    uint64_t current_time_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
        now.time_since_epoch()).count();

    // Query the transform from 1 second ago
    uint64_t target_time_ns = current_time_ns - 1000000000ULL; // 1 second ago

    agibot::gdk::Transform transform;
    if (tf.LookupTransform("base_link", "arm_l_end_link", target_time_ns, transform)
        != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to lookup transform at time " << target_time_ns << std::endl;
    } else {
        std::cout << "Transform from arm_l_end_link to base_link at time " << target_time_ns << ":" << std::endl;
        std::cout << "  Translation: x=" << transform.translation.x
                  << ", y=" << transform.translation.y
                  << ", z=" << transform.translation.z << std::endl;
        std::cout << "  Rotation: x=" << transform.rotation.x
                  << ", y=" << transform.rotation.y
                  << ", z=" << transform.rotation.z
                  << ", w=" << transform.rotation.w << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

6. CanTransform()

• Function: Check whether a transform relationship exists between two coordinate frames
• Parameters:

Parameter	Type	Description
target_frame	const std::string&	Target coordinate frame ID
source_frame	const std::string&	Source coordinate frame ID

• Return value: bool, returns true if a transform relationship exists, otherwise false

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Check whether a transform exists
    if (tf.CanTransform("base_link", "arm_l_end_link")) {
        std::cout << "A transform from arm_l_end_link to base_link exists" << std::endl;
    } else {
        std::cout << "No transform from arm_l_end_link to base_link exists" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

7. GetAllFrameNames()

• Function: Get the names of all available coordinate frames
• Parameters: None

• Return value: std::vector<std::string>, a list containing the names of all coordinate frames

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Get all coordinate frame names
    std::vector<std::string> frame_names = tf.GetAllFrameNames();
    std::cout << "All available coordinate frames (" << frame_names.size() << "):" << std::endl;
    for (const auto& name : frame_names) {
        std::cout << "  - " << name << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

8. GetLatestTimestamp()

• Function: Get the latest timestamp of a specified coordinate frame
• Parameters:

Parameter	Type	Description
frame_id	const std::string&	Coordinate frame ID
timestamp_ns	uint64_t&	Output parameter, latest timestamp (nanoseconds)

• Return value: GDKRes, an operation result status code. Returns GDKRes::kSuccess on success, and the timestamp_ns parameter contains the latest timestamp

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    uint64_t timestamp_ns = 0;
    if (tf.GetLatestTimestamp("arm_l_end_link", timestamp_ns)
        != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get latest timestamp" << std::endl;
    } else {
        std::cout << "arm_l_end_link latest timestamp: " << timestamp_ns << " ns" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

9. Clear()

• Function: Clear all transform relationships in the TF cache
• Parameters: None

• Return value: None (void)

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::TF tf;
    std::cout << "TF init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Clear the TF cache
    tf.Clear();
    std::cout << "TF cache cleared" << std::endl;

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

Usage Notes

1. GDK initialization: agibot::gdk::GDKInit() must be called to initialize the GDK system before using the TF functionality
2. GDK release: agibot::gdk::GDKRelease() must be called to release GDK system resources before the program ends
3. Initialization wait: After creating the TF object, it is recommended to wait 1 second to ensure the DDS connection is established
4. Coordinate frame naming: Use the correct coordinate frame names; all available coordinate frame names can be obtained via GetAllFrameNames()
5. Timestamp precision: The timestamp unit is nanoseconds, which can be used for precise time synchronization
6. Transform matrix: The transform information contains translation and rotation, which can be used for coordinate conversion calculations
7. Sensor calibration: Use GetTfFromSensor() to get sensor extrinsics, for multi-sensor data fusion
8. Real-time behavior: Transform relationships are updated in real time, reflecting the robot's current state
9. Error handling: Always check the GDKRes return value to ensure the operation succeeded
10. Transform queries: LookupTransformLatest() queries the latest transform, while LookupTransform() supports time-interpolated queries of historical transforms
11. Transform checking: Use CanTransform() to check whether a transform exists before querying, to avoid query failures
12. Time interpolation: LookupTransform() supports time interpolation, allowing queries of the transform relationship at any historical moment
13. Cache management: Use Clear() to clear the TF cache, useful for scenarios that need to reset transform relationships
14. Timestamp queries: GetLatestTimestamp() can be used to get the latest update time of a specified coordinate frame

Application Scenarios

• Coordinate transformation: Perform coordinate conversion between different coordinate frames
• Sensor calibration: Obtain sensor extrinsics for multi-sensor calibration
• Multi-sensor fusion: Combine data from multiple sensors to improve perception accuracy
• SLAM mapping: Provide coordinate frame transform information for SLAM algorithms
• Path planning: Account for the coordinate frame relationships of different components when planning paths
• Visual processing: Convert image coordinates into the robot's coordinate frame
• Motion control: Account for coordinate frame transforms when controlling robot motion
• Data synchronization: Synchronize multi-sensor data based on timestamps