GDK SLAM Interface Documentation (C++)

Overview

The SLAM (Simultaneous Localization and Mapping) module provides real-time mapping and localization capabilities for the G02 robot. Through the C++ interface, developers can conveniently implement environment perception, map construction, position estimation, and other functions for the robot, suitable for various scenarios such as autonomous navigation, environment modeling, and localization services.

Interface Description

Slam Class

This class encapsulates the main functional interfaces of the SLAM system.

1. GetSlamState()

• Function: Get the current state of the SLAM system
• Parameters:

Parameter Name	Type	Description
state	uint32_t&	Output parameter, SLAM state code

• Return Value: GDKRes, operation result status code. Returns GDKRes::kSuccess on success; the state parameter contains the SLAM state information (1: mapping started, 2: mapping stopped, 0: mapping cancelled)

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
using namespace agibot::gdk;

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 initialization succeeded" << std::endl;

    Slam slam;
    std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait 1 second to ensure the DDS connection is established
    uint32_t state;
    GDKRes result = slam.GetSlamState(state);

    if (result == GDKRes::kSuccess) {
        std::cout << "SLAM state: " << state << std::endl;
    } else {
        std::cout << "Failed to get SLAM state" << 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 release succeeded" << std::endl;

    return 0;
}

2. StartMapping()

• Function: Start mapping

• Parameters: None

• Return Value: GDKRes, operation result status code. Returns GDKRes::kSuccess on success

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <unistd.h>
#include <chrono>
#include <thread>
using namespace agibot::gdk;

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 initialization succeeded" << std::endl;

    Slam slam;
    std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait 1 second to ensure the DDS connection is established
    // Start mapping
    GDKRes result = slam.StartMapping();

    if (result == GDKRes::kSuccess) {
        std::cout << "Mapping started successfully" << std::endl;

        // Check mapping status
        sleep(2);
        uint32_t state;
        result = slam.GetSlamState(state);
        if (result == GDKRes::kSuccess) {
            std::cout << "Mapping status: " << state << std::endl;
        }
    } else {
        std::cout << "Failed to start mapping" << 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 release succeeded" << std::endl;

    return 0;
}

3. StopMapping()

• Function: Stop mapping and save the map

• Parameters: None

• Return Value: GDKRes, operation result status code. Returns GDKRes::kSuccess on success

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <unistd.h>
#include <chrono>
#include <thread>
using namespace agibot::gdk;

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 initialization succeeded" << std::endl;

    Slam slam;
    std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait 1 second to ensure the DDS connection is established
    // Stop mapping
    GDKRes result = slam.StopMapping();

    if (result == GDKRes::kSuccess) {
        std::cout << "Mapping stopped successfully" << std::endl;

        // Check mapping status
        sleep(2);
        uint32_t state;
        result = slam.GetSlamState(state);
        if (result == GDKRes::kSuccess) {
            std::cout << "Mapping status: " << state << std::endl;
        }
    } else {
        std::cout << "Failed to stop mapping" << 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 release succeeded" << std::endl;

    return 0;
}

4. CancelMapping()

• Function: Cancel mapping

• Parameters: None

• Return Value: GDKRes, operation result status code. Returns GDKRes::kSuccess on success

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <unistd.h>
#include <chrono>
#include <thread>
using namespace agibot::gdk;

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 initialization succeeded" << std::endl;

    Slam slam;
    std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait 1 second to ensure the DDS connection is established
    // Cancel mapping
    GDKRes result = slam.CancelMapping();

    if (result == GDKRes::kSuccess) {
        std::cout << "Mapping cancelled successfully" << std::endl;

        // Check mapping status
        sleep(2);
        uint32_t state;
        result = slam.GetSlamState(state);
        if (result == GDKRes::kSuccess) {
            std::cout << "Mapping status: " << state << std::endl;
        }
    } else {
        std::cout << "Failed to cancel mapping" << 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 release succeeded" << std::endl;

    return 0;
}

5. GetOdomInfo()

• Function: Get odometry information
• Parameters:

Parameter Name	Type	Description
odom_info	OdomInfo&	Output parameter, odometry information object

• Return Value: GDKRes, operation result status code. Returns GDKRes::kSuccess on success; the odom_info parameter contains the odometry information

Detailed Description of the OdomInfo Object

The OdomInfo struct contains the following members:

Member Name	Type	Description	Unit
pose	PoseWithCovariance	Robot's current pose (with covariance)	Pose
twist	TwistWithCovariance	Robot's current velocity (with covariance)	Velocity
is_stationary	bool	Whether the robot is stationary	Boolean
is_sliping	bool	Whether the robot is slipping	Boolean
loc_confidence	int32_t	Localization confidence	No unit
loc_state	int32_t	Localization state	No unit
velocity	Vector3	Robot's current velocity	Meters/second
velocity_body	Vector3	Robot's body-frame velocity	Meters/second
acceleration	Vector3	Robot's current acceleration	Meters/second²
ang_vel	Vector3	Robot's current angular velocity	Radians/second
orientation_euler	Vector3	Robot's current Euler angles	Radians

struct OdomInfo {
  PoseWithCovariance pose{};    ///< robot current pose
  TwistWithCovariance twist{};  ///< robot current twist
  bool is_stationary{};         ///< robot if stationary
  bool is_sliping{};            ///< robot if slipping
  int32_t loc_confidence{};     ///< robot location confidence
  int32_t loc_state{};          ///< robot location state
  Vector3 velocity{};           ///< robot current velocity
  Vector3 velocity_body{};      ///< robot current body velocity
  Vector3 acceleration{};       ///< robot current acceleration
  Vector3 ang_vel{};            ///< robot current angular velocity
  Vector3 orientation_euler{};  ///< robot current orientation euler
};

PoseWithCovariance struct:

Member Name	Type	Description
pose	Pose	Pose information
covariance	std::vector<double>	Covariance matrix

struct PoseWithCovariance {
  Pose pose{};
  std::vector<double> covariance{};
};

TwistWithCovariance struct:

Member Name	Type	Description
twist	Twist	Velocity information
covariance	std::vector<double>	Covariance matrix

struct TwistWithCovariance {
  Twist twist{};
  std::vector<double> covariance{};
};

Pose struct:

Member Name	Type	Description
position	Position	Position information
orientation	Orientation	Orientation information

struct Pose {
  Position position{};
  Orientation orientation{};
};

Position struct:

Member Name	Type	Description	Unit
x	double	X coordinate	Meters
y	double	Y coordinate	Meters
z	double	Z coordinate	Meters

Orientation struct:

Member Name	Type	Description	Unit
x	double	Quaternion X component	No unit
y	double	Quaternion Y component	No unit
z	double	Quaternion Z component	No unit
w	double	Quaternion W component	No unit

Twist struct:

Member Name	Type	Description
linear	Vector3	Linear velocity
angular	Vector3	Angular velocity

struct Twist {minear{};   ///< linear velocity (m/s)
  Vector3 angular{};  ///< angular velocity (rad/s)
};

Vector3 struct:

Member Name	Type	Description	Unit
x	double	X-axis component	Meters/second or radians/second
y	double	Y-axis component	Meters/second or radians/second
z	double	Z-axis component	Meters/second or radians/second

• Example:

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

  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 initialization succeeded" << std::endl;

      agibot::gdk::Slam slam;
      std::cout << "Slam init" << std::endl;
      std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait for SLAM initialization to complete

      slam.StartMapping();
      std::cout << "Start mapping" << std::endl;
      for(int i = 0; i < 10; i++)
      {
          agibot::gdk::OdomInfo odom;
          if (slam.GetOdomInfo(odom) != agibot::gdk::GDKRes::kSuccess) {
              std::cout << "Failed to get odom info" << std::endl;
          } else {
              std::cout << "pose: (" << odom.pose.pose.position.x << ", " << odom.pose.pose.position.y << ", " << odom.pose.pose.position.z << ")" << std::endl;
              std::cout << "orientation (quaternion): x=" << odom.pose.pose.orientation.x 
                      << ", y=" << odom.pose.pose.orientation.y 
                      << ", z=" << odom.pose.pose.orientation.z 
                      << ", w=" << odom.pose.pose.orientation.w << std::endl;
          }
          std::this_thread::sleep_for(std::chrono::seconds(1));
      }
      slam.StopMapping();

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

      return 0;
  }

6. RecordSpecLoc()

• Function: Record the current position as a specific location

• Parameters: None

• Return Value: GDKRes, operation result status code. Returns GDKRes::kSuccess on success

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
using namespace agibot::gdk;

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 initialization succeeded" << std::endl;

    Slam slam;
    std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait 1 second to ensure the DDS connection is established
    GDKRes result = slam.RecordSpecLoc();

    if (result == GDKRes::kSuccess) {
        std::cout << "Specific location recorded successfully" << std::endl;
    } else {
        std::cout << "Failed to record specific location" << 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 release succeeded" << std::endl;

    return 0;
}

7. GetCurrPose()

• Function: Get the robot's current position
• Parameters:

Parameter Name	Type	Description
pose	Pose&	Output parameter, robot's current pose

• Return Value: GDKRes, operation result status code. Returns GDKRes::kSuccess on success; the pose parameter contains the robot's current pose

• Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
using namespace agibot::gdk;

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 initialization succeeded" << std::endl;

    Slam slam;
    std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait 1 second to ensure the DDS connection is established

    Pose current_pose;
    GDKRes result = slam.GetCurrPose(current_pose);

    if (result == GDKRes::kSuccess) {
        std::cout << "Current position: (" << current_pose.position.x 
                  << ", " << current_pose.position.y 
                  << ", " << current_pose.position.z << ")" << std::endl;
        std::cout << "Current orientation: x=" << current_pose.orientation.x 
                  << ", y=" << current_pose.orientation.y 
                  << ", z=" << current_pose.orientation.z 
                  << ", w=" << current_pose.orientation.w << std::endl;
    } else {
        std::cout << "Failed to get current position" << 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 release succeeded" << std::endl;

    return 0;
}

8. Complete Usage Example

• Function: Demonstrates the complete usage workflow of the SLAM module
• Example:

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

  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 initialization succeeded" << std::endl;

      agibot::gdk::Slam slam;
      agibot::gdk::Map map;
      std::cout << "Slam init" << std::endl;
      std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait for SLAM initialization to complete

      slam.StartMapping();
      std::cout << "Start mapping" << std::endl;
      for(int i = 0; i < 10; i++)
      {
          agibot::gdk::OdomInfo odom;
          if (slam.GetOdomInfo(odom) != agibot::gdk::GDKRes::kSuccess) {
              std::cout << "Failed to get odom info" << std::endl;
          } else {
              std::cout << "pose: (" << odom.pose.pose.position.x << ", " << odom.pose.pose.position.y << ", " << odom.pose.pose.position.z << ")" << std::endl;
              std::cout << "orientation (quaternion): x=" << odom.pose.pose.orientation.x 
                      << ", y=" << odom.pose.pose.orientation.y 
                      << ", z=" << odom.pose.pose.orientation.z 
                      << ", w=" << odom.pose.pose.orientation.w << std::endl;
          }
          uint32_t state;
          if (slam.GetSlamState(state) != agibot::gdk::GDKRes::kSuccess) {
              std::cout << "Failed to get slam state" << std::endl;
          } else {
              std::cout << "slam state: " << state << std::endl;
          }

          std::this_thread::sleep_for(std::chrono::seconds(1));
      }
      agibot::gdk::MapName map_name;
      map.GetCurrMap(map_name);
      std::cout << "Get current map: " << map_name.id << std::endl;

      std::vector<agibot::gdk::MapName> map_names;
      map.GetAllMap(map_names);
      for (const auto& name : map_names) {
          std::cout << "Map: " << name.id << ", " << name.name << ", is current: " << name.is_curr_map << std::endl;
      }
      slam.StopMapping();

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

      return 0;
  }

Usage Notes

1. GDK initialization: Before using SLAM functionality, you must first call agibot::gdk::GDKInit() to initialize the GDK system
2. GDK release: Before the program ends, you must call agibot::gdk::GDKRelease() to release GDK system resources
3. Initialization wait: After creating a Slam object, it is recommended to wait 1 second to ensure the DDS connection is established
4. Mapping order: It is recommended to start mapping first before performing other operations
5. State checking: It is recommended to check the SLAM state before and after operations to ensure success
6. Odometry information: Ensure SLAM or PNC is running when getting odometry information
7. Relocalization: Global relocalization must be performed after map construction is complete
8. Resource management: Pay attention to the SLAM system's resource usage and avoid excessive requests
9. Error handling: Always check the GDKRes return value to ensure the operation succeeded

Application Scenarios

• Environment mapping: Build a map of the robot's working environment
• Real-time localization: Get the robot's real-time position within the environment
• Navigation support: Provide localization and map data for the navigation system
• Environment modeling: Build a spatial model of the environment
• Position recording: Record and mark important locations
• Multi-map management: Support map construction and management for multiple environments