C++ 示例

本文件为 API 中公开的函数与类型提供简短示例，演示如何使用这些接口。

机器人关节名称（G1 2.2）

关节组列表

机器人关节组名称包括：["head", "left_arm", "right_arm", "leg", "left_gripper", "right_gripper", "left_suction_cup", "right_suction_cup"]

各关节组详细信息

关节组名称	关节组英文名	关节数量	关节名称列表
头部	head	2	head_joint1, head_joint2
腿部	leg	5	leg_joint1, leg_joint2, leg_joint3, leg_joint4, leg_joint5
左臂	left_arm	7	left_arm_joint1, left_arm_joint2, left_arm_joint3, left_arm_joint4, left_arm_joint5, left_arm_joint6, left_arm_joint7
右臂	right_arm	7	right_arm_joint1, right_arm_joint2, right_arm_joint3, right_arm_joint4, right_arm_joint5, right_arm_joint6, right_arm_joint7
左夹爪	left_gripper	1	left_gripper_joint1
右夹爪	right_gripper	1	right_gripper_joint1
左吸盘	left_suction_cup	1	left_suction_cup_joint1
右吸盘	right_suction_cup	1	right_suction_cup_joint1

如何正确使用 joint_groups

joint_groups 的设计单位是“关节组（运动链）”，而不是单个关节。这样设计是为了保证：

• 运动链一致性：头/臂/腿等关节按链路整体校验和控制。
• 指令顺序确定性：joint_groups 会按传入顺序展开为具体关节。
• 组级约束校验：执行前会检查关节组的 active/passive 属性及输入合法性。

传参规则：

1. 需要控制整条链路时，优先使用 joint_groups（head/arm/leg/gripper/suction cup）。
2. 当 joint_names 非空时，优先使用 joint_names，会覆盖 joint_groups。
3. 当 joint_groups 和 joint_names 都为空时，SDK 默认控制所有 active body 关节组。
4. 建议先调用 get_joint_group_names() 和 get_joint_names(true, groups)，避免手写字符串出错。

G1 关节组典型场景

关节组	典型场景
head	头部朝向/相机对准
left_arm / right_arm	机械臂抓取与操作
leg	下肢姿态调整
left_gripper / right_gripper	夹爪开合宽度控制
left_suction_cup / right_suction_cup	吸盘吸放作业

传感器类型与 Frame 说明

获取传感器数据时（get_rgb_data、get_depth_data、get_imu_data、get_lidar_data），通过 SensorType 枚举指定要查询的传感器。可用的 SensorType 枚举值详见：C++ API 参考 > 传感器类型。

获取传感器外参有两种方式：

• get_sensor_extrinsic(): SDK 内部已映射 frame ID，直接传入 SensorType 即可
• get_transform(): 需要显式传入 frame 名称，表格第二列为各传感器对应的 Frame ID

SensorType	Frame ID
HEAD_LEFT_CAMERA	head_left_camera_color_optical_frame
HEAD_RIGHT_CAMERA	head_right_camera_color_optical_frame
LEFT_ARM_CAMERA	left_arm_camera_color_optical_frame
LEFT_ARM_DEPTH_CAMERA	left_arm_camera_color_optical_frame
RIGHT_ARM_CAMERA	right_arm_camera_color_optical_frame
RIGHT_ARM_DEPTH_CAMERA	right_arm_camera_color_optical_frame
BASE_LIDAR	lidar_base_link
TORSO_IMU	imu_base_link
BASE_ULTRASONIC	—（无 TF frame）
LEFT_FRONT_SURROUND_CAMERA	left_front_surround_color_optical_frame
RIGHT_FRONT_SURROUND_CAMERA	right_front_surround_color_optical_frame
LEFT_REAR_SURROUND_CAMERA	left_rear_surround_color_optical_frame
RIGHT_REAR_SURROUND_CAMERA	right_rear_surround_color_optical_frame

注意：调用 get_frame_names() 可获取当前所有可用的坐标系名称。

类：GalbotRobot

tips：程序启动后立刻就get，数据可能不会立刻就绪，可适当sleep几秒

获取实例并初始化（get_instance && init）

适用场景：程序启动阶段，获取机器人单例并完成 SDK 初始化。必须在调用其他 API 前执行。

examples/cpp/galbot_robot/src/get_instance_init_example.cpp

#include <iostream>
#include <vector>
#include <array>
#include <memory>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    
    // Initialize singleton instance
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Check whether it is in the running state
    while (robot.is_running()) {
        // do something
        std::cout << "System is running." << std::endl;
        break;
    }

    // Register an exit callback (optional; automatically triggered when an exit signal is received)
    robot.register_exit_callback([]() {
        std::cout << "System is exiting..." << std::endl;
    });
    std::cout << "System exit callback registered successfully" << std::endl;

    // Send exit signal
    robot.request_shutdown();
    // Wait until entering shutdown state
    robot.wait_for_shutdown();
    // Release SDK related resources
    robot.destroy();
    std::cout << "Program finished" << std::endl;

    return 0;
}

释放SDK相关资源并退出程序

适用场景：程序退出前释放 SDK 占用的所有资源，正常关闭系统。

examples/cpp/galbot_robot/src/get_instance_init_example.cpp

#include <iostream>
#include <vector>
#include <array>
#include <memory>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    
    // Initialize singleton instance
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Check whether it is in the running state
    while (robot.is_running()) {
        // do something
        std::cout << "System is running." << std::endl;
        break;
    }

    // Register an exit callback (optional; automatically triggered when an exit signal is received)
    robot.register_exit_callback([]() {
        std::cout << "System is exiting..." << std::endl;
    });
    std::cout << "System exit callback registered successfully" << std::endl;

    // Send exit signal
    robot.request_shutdown();
    // Wait until entering shutdown state
    robot.wait_for_shutdown();
    // Release SDK related resources
    robot.destroy();
    std::cout << "Program finished" << std::endl;

    return 0;
}

日志函数

适用场景：使用 SDK 内置的日志系统输出日志，统一日志级别和格式。

examples/cpp/galbot_robot/src/logger_example.cpp

#include <iostream>

#include "galbot_robot.hpp"
#include "galbot_sdk_logger.hpp"

using namespace galbot::sdk;

int main() {
  // Get and initialize the GalbotRobot singleton
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  robot.init();
  // ==============================
  // 1. Initialize SDK logging
  // ==============================
  LoggerConfig logger_config;
  // Log generation path; if not filled, defaults to ~/galbot_sdk_log/user_log
  logger_config.path = "";
  // Log file name; if not filled, defaults to <process_name>_<current_time>_<pid>_<thread_id>.log
  logger_config.file_name = "";
  // Maximum bytes per single log file
  logger_config.file_max_size = 1024 * 1024 * 10; // 10MB
  // Maximum number of rotating log files; creates new file when full, retaining up to file_max_num files
  logger_config.file_max_num = 5;
  // Minimum log output level
  logger_config.level = LogLevel::DEBUG;
  // Whether to output to terminal; default is false
  logger_config.console_output = false;

  if (!init_galbot_sdk_logger(logger_config)) {
    std::cerr << "failed to init galbot sdk logger" << std::endl;
    return -1;
  } else {
    std::cout << "galbot sdk logger initialized successfully" << std::endl;
  }

  // ==============================
  // 2. Print logs at different levels
  // ==============================
  GALBOT_SDK_LOG_TRACE << "this is trace log";
  GALBOT_SDK_LOG_DEBUG << "this is debug log";
  GALBOT_SDK_LOG_INFO << "this is info log";
  GALBOT_SDK_LOG_WARN << "this is warning log";
  GALBOT_SDK_LOG_ERROR << "this is error log";
  GALBOT_SDK_LOG_CRITICAL << "this is critical log";

  // ==============================
  // 3. Supports chained << style
  // ==============================
  int robot_id = 3;
  GALBOT_SDK_LOG_INFO << "robot_id = " << robot_id;

  GALBOT_SDK_LOG_INFO << "example program finished";

  // Exit the system and release SDK and logger resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

设置关节角度（set_joint_positions）

适用场景：单次点位移动，低频率的位置控制任务。该接口内部会对目标位置进行速度受限的插值规划，适合一次性移动到目标关节角度的场景。

WARNING: 此接口不适合模型推理输出的高频关节控制场景！该接口每次调用都会产生一次轨迹插值，连续调用会导致运动不连续且有延迟。

如果您在做模型推理场景，请使用 set_joint_commands 或 set_joint_commands_batch 直接下发关节指令。

examples/cpp/galbot_robot/src/set_joint_positions_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get specified joint names; joint groups include ["leg", "head", "left_arm", "right_arm"]
    std::vector<std::string> joint_groups = {"head"};
    bool only_active_joint = true;  // Get active joints
    auto head_joint_names_vec =
        robot.get_joint_names(only_active_joint, joint_groups);
    std::cout << "Head joint names:" << std::endl;
    for (size_t i = 0; i < head_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << head_joint_names_vec[i] << std::endl;
    }
    // Passing an empty array returns all joint group information by default
    std::vector<std::string> null_vec = {};
    auto all_joint_names_vec =
        robot.get_joint_names(only_active_joint, null_vec);
    std::cout << "All joint names:" << std::endl;
    for (size_t i = 0; i < all_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << all_joint_names_vec[i] << std::endl;
    }

    // Joint groups to control; passing an empty array controls leg, head, left_arm, and right_arm by default
    joint_groups = {"head"};
    // Specific joints to control; if provided, this overrides the joint_groups parameter
    std::vector<std::string> joint_names = {};
    // Joint positions; head joint group contains two joints
    std::vector<double> joint_pos = {0.2, 0.2};
    // Whether to block and wait for joint angles to reach position or timeout
    bool is_block = true;
    // Maximum joint speed (rad/s)
    double speed_rad_s = 0.1;
    // Maximum wait time (seconds)
    double timeout_s = 10.0;

    // Set joint positions
    ControlStatus joint_execution_status =
        robot.set_joint_positions(joint_pos, joint_groups,joint_names, 
            is_block, speed_rad_s,timeout_s);

    if (joint_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Joint command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set joint command!" << std::endl;
    }

    // Query joint positions by group; empty array defaults to leg, head, dual-arm groups. Second parameter specifies joint names, which overrides joint_groups if provided.
    auto ret_positions = robot.get_joint_positions(joint_groups, {});
    for (auto position : ret_positions) {
        std::cout << "joint positions is " << position << std::endl;
    }

    // Use specific joint names for control; this parameter overrides joint_groups
    joint_names = {"head_joint1", "head_joint2"};
    joint_pos = {0.0, 0.0};

    // Set joint positions
    joint_execution_status = robot.set_joint_positions(joint_pos, joint_groups,joint_names, 
            is_block, speed_rad_s,timeout_s);

    if (joint_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Joint command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set joint command!" << std::endl;
    }

    // Query joint positions by group; empty array defaults to leg, head, dual-arm groups. Second parameter specifies joint names, which overrides joint_groups if provided.
    ret_positions = robot.get_joint_positions(joint_groups, {});
    for (auto position : ret_positions) {
        std::cout << "joint positions is " << position << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

设置夹爪指令（set_gripper_command）

适用场景：控制夹爪开合。支持位置控制和力矩控制两种模式。

examples/cpp/galbot_robot/src/set_gripper_command_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_gripper_state(std::shared_ptr<GripperState> gripper_state) {
    std::cout << "Timestamp (ns): " << gripper_state->timestamp_ns << std::endl;

    std::cout << " width "  << gripper_state->width << " velocity " << gripper_state->velocity
                << " effort " << gripper_state->effort << " is moving "
                << gripper_state->is_moving << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Gripper width (m)
    double width_m = 0.02;
    // Gripper speed (m/s)
    double velocity_mps = 0.05;
    // Gripper torque (N·m)
    double effort = 10;
    // Whether to block until execution completes
    bool is_blocking = true;
    // Set left gripper width to 0.02m, speed 0.05m/s, torque 10, block until execution completes
    ControlStatus gripper_execution_status =
        robot.set_gripper_command("left_gripper", width_m, velocity_mps,
                                        effort, is_blocking);

    if (gripper_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Gripper command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set gripper command!" << std::endl;
    }

    // Get gripper state
    JointStateMessage joint_state;
    auto gripper_state_ptr = robot.get_gripper_state("left_gripper");

    if (gripper_state_ptr == nullptr) {
        std::cerr << "get gripper state error" << std::endl;
    } else {
        print_gripper_state(gripper_state_ptr);
    }

    // Gripper width (m)
    width_m = 0.1;
    // Gripper speed (m/s)
    velocity_mps = 0.05;
    // Gripper torque (N·m)
    effort = 10;
    // Whether to block until execution completes
    is_blocking = false;
    // Set left gripper width to 0.1m, speed 0.05m/s, torque 10, block until execution completes
    gripper_execution_status =
        robot.set_gripper_command("left_gripper", width_m, velocity_mps,
                                        effort, is_blocking);

    if (gripper_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Gripper command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set gripper command!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

设置吸盘指令（set_suction_cup_command）

适用场景：控制吸盘吸放物体，获取吸盘当前的开启/关闭状态。

examples/cpp/galbot_robot/src/set_suction_cup_command_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Activate suction cup
    if (robot.set_suction_cup_command("right_suction_cup", true) == ControlStatus::SUCCESS) {
        std::cout << "Suction cup activation command sent successfully" << std::endl;
        
    } else {
        std::cerr << "Suction cup activation command failed to send!" << std::endl;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    // Deactivate suction cup
    if (robot.set_suction_cup_command("right_suction_cup", false) == ControlStatus::SUCCESS) {
        std::cout << "Suction cup deactivation command sent successfully" << std::endl;
        
    } else {
        std::cerr << "Suction cup deactivation command failed to send" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

停止轨迹执行（stop_trajectory_execution）

适用场景：停止当前正在执行的关节轨迹，中断正在进行的运动。

examples/cpp/galbot_robot/src/stop_trajectory_execution_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Stop trajectory execution
    while(true) {
        ControlStatus joint_execution_status =
            robot.stop_trajectory_execution();
        
        // Check execution results
        if (joint_execution_status == ControlStatus::SUCCESS) {
            std::cout << "Trajectory stop command sent successfully" << std::endl;
            break;
        } else {
            std::cerr << "Failed to send trajectory stop command, retrying..." << std::endl;
        }
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

设置轨迹（execute_joint_trajectory）

适用场景：执行一条预定义的关节空间轨迹，包含多个路点。SDK 会按照时间节点执行整条轨迹。

examples/cpp/galbot_robot/src/execute_joint_trajectory_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

double g_target_time = 10;
double g_start_time = 10;

std::string trajectory_status_to_string(TrajectoryControlStatus status) {
  switch (status) {
  case TrajectoryControlStatus::INVALID_INPUT:
    return "INVALID_INPUT";
  case TrajectoryControlStatus::RUNNING:
    return "RUNNING";
  case TrajectoryControlStatus::COMPLETED:
    return "COMPLETED";
  case TrajectoryControlStatus::STOPPED_UNREACHED:
    return "STOPPED_UNREACHED";
  case TrajectoryControlStatus::ERROR:
    return "ERROR";
  case TrajectoryControlStatus::DATA_FETCH_FAILED:
    return "DATA_FETCH_FAILED";
  case TrajectoryControlStatus::STATUS_NUM:
    return "STATUS_NUM";
  default:
    return "UNKNOWN_STATUS";
  }
}

void wait_for_traj_reached(const std::vector<std::string> &joint_groups) {
    std::vector<TrajectoryControlStatus> traj_exec_states;
    int count = 0;
    bool all_reached = false;
    while (count++ < 150) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        all_reached = true;
        traj_exec_states = GalbotRobot::get_instance(MachineType::G1)
                            .check_trajectory_execution_status(joint_groups);
        if (traj_exec_states.size() != joint_groups.size()) {
        std::cout << "traj_exec_states size != joint_groups size" << std::endl;
        }
        for (int i = 0; i < joint_groups.size(); ++i) {
        std::cout << joint_groups[i] << " exec state is "
                    << trajectory_status_to_string(traj_exec_states[i])
                    << std::endl;
        if (traj_exec_states[i] != TrajectoryControlStatus::COMPLETED) {
            all_reached = false;
        }
        }

        if (all_reached) {
            std::cout << "all reached" << std::endl;
            break;
        }
    }
    for (const auto &status : traj_exec_states) {
        std::cout << "done reached state is " << trajectory_status_to_string(status)
                << std::endl;
    }
}

std::vector<TrajectoryPoint>
generate_target_trajectory(int32_t joint_size, double ampl = 0.2,
                           double cycle = 10) {
  double amplitude = -ampl;
  double frequency = 1.0 / cycle;
  double phase = -M_PI / 2;
  double offset = amplitude;
  double dt = 0.004;
  int step = g_target_time / dt;

  std::vector<TrajectoryPoint> trajectory_data_vec;
  trajectory_data_vec.resize(step + 1);
  // Create a RobotCommand trajectory
  for (int i = 0; i <= step; ++i) {
    double t = i * dt;
    trajectory_data_vec[i].time_from_start_second = g_start_time + t;
    trajectory_data_vec[i].joint_command_vec.resize(joint_size);
    // Joint command
    for (int j = 0; j < joint_size; ++j) {
      trajectory_data_vec[i].joint_command_vec[j].position =
          offset + amplitude * std::sin(2 * M_PI * frequency * t + phase);
      trajectory_data_vec[i].joint_command_vec[j].velocity =
          amplitude * 2 * M_PI * frequency *
          std::cos(2 * M_PI * frequency * t + phase);
    }
  }

  return trajectory_data_vec;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Execute joint trajectory
    Trajectory trajectory;
    // Enter joint group name to control, including ["leg", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]
    trajectory.joint_groups = {"head"};
    // Fill this field to control specific joint angles, which will override joint_groups if provided
    trajectory.joint_names = {};
    // Generate trajectory
    trajectory.points = generate_target_trajectory(2);
    // Whether to block until trajectory execution completes; when false, you can use
    bool is_traj_block = false;

    // Wait for trajectory execution to complete; this function wraps check_trajectory_execution_status to check trajectory execution status
    wait_for_traj_reached(trajectory.joint_groups);

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

设置关节指令（set_joint_commands）

适用场景：高频关节控制，例如模型推理输出（每步推理输出一帧关节指令）、自定义轨迹跟踪。直接下发关节指令给底层控制器，不做额外插值。

examples/cpp/galbot_robot/src/set_joint_command_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_joint_positions(const std::vector<double>& positions) {
    std::cout << "Current joint positions:" << std::endl;
    for (size_t i = 0; i < positions.size(); ++i) {
        std::cout << "  joint " << i << ": " << positions[i] << " rad" << std::endl;
    }
    std::cout << std::endl;
}

std::string execution_status_to_string(ControlStatus status) {
    switch (status) {
        case ControlStatus::SUCCESS:
            return "SUCCESS";
        case ControlStatus::TIMEOUT:
            return "TIMEOUT";
        case ControlStatus::FAULT:
            return "FAULT";
        case ControlStatus::INVALID_INPUT:
            return "INVALID_INPUT";
        case ControlStatus::INIT_FAILED:
            return "INIT_FAILED";
        case ControlStatus::IN_PROGRESS:
            return "IN_PROGRESS";
        case ControlStatus::STOPPED_UNREACHED:
            return "STOPPED_UNREACHED";
        case ControlStatus::DATA_FETCH_FAILED:
            return "DATA_FETCH_FAILED";
        case ControlStatus::PUBLISH_FAIL:
            return "PUBLISH_FAIL";
        case ControlStatus::COMM_DISCONNECTED:
            return "COMM_DISCONNECTED";
        default:
            return "UNKNOWN_STATUS";
    }
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    std::vector<std::string> joint_groups = {"head"};
    std::vector<std::string> joint_names = {};

    std::vector<JointCommand> joint_commands(2);
    joint_commands[0].position = 0.2;
    joint_commands[1].position = 0.2;
    // Set head joint angles to 0.3 0.3
    ControlStatus execution_status =
        robot.set_joint_commands(joint_commands, joint_groups, joint_names);
    if (execution_status != ControlStatus::SUCCESS) {
        std::cout << "Joint angle command sending failed" << std::endl;
    } else {
        std::cout << "Joint angle command sent successfully" << std::endl;
    }

    // , waitexecute
    std::this_thread::sleep_for(std::chrono::milliseconds(10000));

    // Query joint positions
    auto ret_positions = robot.get_joint_positions(joint_groups, {});
    print_joint_positions(ret_positions);

    // Step 2: Return to initial position —— set both head joints to 0.0 rad
    joint_commands[0].position = 0.0;
    joint_commands[1].position = 0.0;

    // Set joint commands by joint names; setting joint_names overrides joint_groups
    joint_groups = {""};
    joint_names = {"head_joint1", "head_joint2"};
    execution_status =
        robot.set_joint_commands(joint_commands, joint_groups, joint_names);
    if (execution_status != ControlStatus::SUCCESS) {
        std::cout << "Joint angle command sending failed" << std::endl;
    } else {
        std::cout << "Joint angle command sent successfully" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    std::cout << "\nProgram exited" << std::endl;
    return 0;
}

批量设置关节指令（set_joint_commands_batch）

适用场景：批量下发未来多帧关节指令，用于运动预测模型一次性输出多步结果的场景，可以提高控制频率和连续性。

examples/cpp/galbot_robot/src/set_joint_commands_batch_example.cpp

#include <chrono>
#include <cmath>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    default:
      return "UNKNOWN_STATUS";
  }
}

std::vector<TrajectoryPoint> generate_batch_trajectory(int32_t joint_size, double ampl = 0.2, double cycle = 10,
                                                       int num_points = 10) {
  double amplitude = -ampl;
  double frequency = 1.0 / cycle;
  double phase = -M_PI / 2;
  double offset = amplitude;
  double dt = cycle / num_points;

  std::vector<TrajectoryPoint> trajectory_data_vec;
  trajectory_data_vec.resize(num_points);
  // Create batch trajectory points
  for (int i = 0; i < num_points; ++i) {
    double t = i * dt;
    trajectory_data_vec[i].time_from_start_second = t;
    trajectory_data_vec[i].joint_command_vec.resize(joint_size);
    // Joint command
    for (int j = 0; j < joint_size; ++j) {
      trajectory_data_vec[i].joint_command_vec[j].position =
          offset + amplitude * std::sin(2 * M_PI * frequency * t + phase);
      trajectory_data_vec[i].joint_command_vec[j].velocity =
          amplitude * 2 * M_PI * frequency * std::cos(2 * M_PI * frequency * t + phase);
      // trajectory_data_vec[i].joint_command_vec[j].acceleration = 0.0;
      // trajectory_data_vec[i].joint_command_vec[j].effort = 0.0;
    }
  }

  return trajectory_data_vec;
}

int main() {
  // Get object instance
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize system
  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

  // Batch set joint commands
  Trajectory trajectory;
  // Enter joint group names to control, including ["leg", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]
  trajectory.joint_groups = {"head"};
  // Fill this field to control specific joint angles, which will override joint_groups if provided
  trajectory.joint_names = {};
  // Generate batched trajectory points (joint commands at multiple time points)
  trajectory.points = generate_batch_trajectory(2, 0.2, 10.0, 10);

  // Batch set joint commands (non-blocking, returns immediately)
  ControlStatus status = robot.set_joint_commands_batch(trajectory);
  std::cout << "Batch joint command status: " << control_status_to_string(status) << std::endl;

  if (status == ControlStatus::SUCCESS) {
    std::cout << "Batch commands submitted, executing in background (non-blocking)" << std::endl;
  } else {
    std::cerr << "Failed to submit batch commands!" << std::endl;
  }

  // Wait for a while to let the command execute
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // Exit system and release SDK resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

直接发布原始目标（PublishTarget）

适用场景：高级用户直接构造 SingoriXTarget 并通过 publish 通道下发到底层 WBCS，适合高频原始 target 发送与 joint/task mixed target 一次性 dispatch。

examples/cpp/galbot_robot/src/publish_target_example.cpp

/**
 * @file publish_target_example.cpp
 * @brief G1 PublishTarget menu example for SDK mirror SingoriXTarget.
 *
 * This example shows how to construct `SingoriXTarget` directly at the SDK layer
 * and send it to the low-level WBCS through `PublishTarget()`.
 *
 * 1. Joint-space commands are written into `target_group_trajectory_map`
 *    - Typical for head / arm style joint-space control
 *    - Each group corresponds to one `TargetGroupTrajectory`
 *    - Each trajectory point is described by `GroupCommand + JointCommand`
 *
 * 2. Chassis pose / twist style task-space commands are written into
 *    `target_task_trajectory_map`
 *    - Typical for chassis pose / chassis twist control
 *    - Each task corresponds to one `TargetTaskTrajectory`
 *    - Each trajectory point is described by `TaskCommand + FrameTriad`
 *
 * 3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
 *    - This supports one-shot dispatch for whole-body / mixed control
 *
 * 4. The current SDK does not automatically switch the chassis controller when
 *    calling `PublishTarget()` / `RequestTarget()`
 *    - Therefore this example explicitly calls
 *      `switch_controller(G1ControllerName::CHASSIS_POSE_CTRL)` or
 *      `switch_controller(G1ControllerName::CHASSIS_TWIST_CTRL)` before
 *      chassis pose / twist / mixed scenes
 *
 * 5. For the base twist scene, this example automatically sends a zero-twist
 *    target after the configured duration to stop the chassis.
 */

#include <chrono>
#include <cstdint>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr const char* kChassisTaskName = "chassis";
constexpr const char* kChassisSubtaskPose = "chassis_pose";
constexpr const char* kChassisSubtaskTwist = "chassis_twist";

int64_t now_ns() {
  return std::chrono::duration_cast<std::chrono::nanoseconds>(
             std::chrono::system_clock::now().time_since_epoch())
      .count();
}

const char* to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

Quaternion yaw_to_quaternion(double yaw) {
  Quaternion q;
  q.z = std::sin(yaw * 0.5);
  q.w = std::cos(yaw * 0.5);
  return q;
}

SingoriXTarget make_empty_target() {
  SingoriXTarget target;
  target.header.timestamp_ns = now_ns();
  target.header.frame_id = "base_link";
  return target;
}

TargetConfig make_group_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_DEFAULT;
  config.target_type = TARGET_TYPE_PROVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DEFAULT;
  config.target_priority = 1;
  return config;
}

TargetConfig make_pose_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_POSE;
  // config.target_type = TARGET_TYPE_PROVERRIDE;
  config.target_type = TARGET_TYPE_OVERRIDE; // single point, override
  config.target_sampling = TargetSampling::TARGET_SAMPLING_LINEAR_INTERPOLATE;
  config.target_priority = 1;
  return config;
}

TargetConfig make_twist_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_TWIST;
  config.target_type = TARGET_TYPE_OVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DIRECT_PASS;
  config.target_priority = 1;
  return config;
}

SingoriXTarget build_joint_target(const std::string& group_name,
                                  const std::vector<std::string>& joint_names,
                                  const std::vector<double>& positions,
                                  double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& group_traj = target.target_group_trajectory_map[group_name];
  group_traj.target_config = make_group_target_config();
  group_traj.joint_names = joint_names;

  GroupCommand command;
  command.time_from_start_s = time_from_start_s;
  command.joint_commands.resize(joint_names.size());
  for (size_t i = 0; i < joint_names.size(); ++i) {
    command.joint_commands[i].position = positions[i];
    command.joint_commands[i].velocity = 0.0;
    command.joint_commands[i].acceleration = 0.0;
    command.joint_commands[i].effort = 0.0;
  }
  group_traj.group_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_pose_target(double x,
                                         double y,
                                         double yaw,
                                         double time_from_start_s,
                                         const std::string& frame_id = "rel(0)", // "rel(0)" means relative to the current base_link pose
                                         const std::string& reference_frame_id = "odom") {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_pose_target_config();
  task_traj.group_names = {G1JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskPose) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = frame_id;
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = reference_frame_id;
  triad.pose = Pose();
  triad.pose->position.x = x;
  triad.pose->position.y = y;
  triad.pose->position.z = 0.0;
  triad.pose->orientation = yaw_to_quaternion(yaw);
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_twist_target(double vx,
                                          double vy,
                                          double wz,
                                          double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_twist_target_config();
  task_traj.group_names = {G1JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskTwist) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = "base_link";
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = "base_link";
  triad.twist = Twist();
  triad.twist->linear = Vector3{vx, vy, 0.0};
  triad.twist->angular = Vector3{0.0, 0.0, wz};
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_stop_twist_target() {
  return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1);
}

SingoriXTarget merge_targets(const std::vector<SingoriXTarget>& targets) {
  SingoriXTarget merged = make_empty_target();
  for (const auto& target : targets) {
    for (const auto& [group_name, group_traj] : target.target_group_trajectory_map) {
      merged.target_group_trajectory_map[group_name] = group_traj;
    }
    for (const auto& [task_name, task_traj] : target.target_task_trajectory_map) {
      merged.target_task_trajectory_map[task_name] = task_traj;
    }
  }
  return merged;
}

void print_menu() {
  std::cout << "\nAvailable commands:\n"
            << "  joint        - publish a joint-only head target\n"
            << "  base_pose    - publish a chassis pose target\n"
            << "  base_twist   - publish a chassis twist target with auto stop\n"
            << "  mixed_pose   - publish head + left_arm + chassis pose in one target\n"
            << "  mixed_twist  - publish head + left_arm + chassis twist in one target\n"
            << "  quit         - exit example\n"
            << std::endl;
}

ControlStatus ensure_controller(GalbotRobot& robot, const std::string& controller_name) {
  const auto status = robot.switch_controller(controller_name);
  std::cout << "switch_controller(" << controller_name << "): " << to_string(status) << std::endl;
  return status;
}

void print_result(const std::string& scene_name, ControlStatus status) {
  std::cout << scene_name << " PublishTarget status: " << to_string(status) << std::endl;
}

ControlStatus run_twist_scene(GalbotRobot& robot,
                              const std::string& scene_name,
                              const SingoriXTarget& target,
                              double twist_duration_s) {
  std::cout << scene_name << ": start moving for " << twist_duration_s << " seconds" << std::endl;
  const auto motion_status = robot.PublishTarget(target);
  print_result(scene_name, motion_status);
  if (motion_status != ControlStatus::SUCCESS) {
    return motion_status;
  }

  std::this_thread::sleep_for(std::chrono::duration<double>(twist_duration_s));
  std::cout << scene_name << ": send stop twist target" << std::endl;
  const auto stop_status = robot.PublishTarget(build_stop_twist_target());
  std::cout << scene_name << " stop status: " << to_string(stop_status) << std::endl;
  return stop_status;
}

}  // namespace

int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::seconds(2));

  const auto head_joint_names = robot.get_joint_names(true, {G1JointGroup::HEAD});
  const auto left_arm_joint_names = robot.get_joint_names(true, {G1JointGroup::LEFT_ARM});
  if (head_joint_names.empty() || left_arm_joint_names.empty()) {
    std::cerr << "failed to fetch active head/left_arm joints" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const std::vector<std::string> head_single_joint = {head_joint_names.front()};
  const std::vector<std::string> arm_single_joint = {left_arm_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;

  print_menu();

  std::string command;
  while (true) {
    std::cout << "Enter command: ";
    if (!(std::cin >> command)) {
      break;
    }
    if (command == "quit") {
      break;
    }

    if (command == "joint") {
      const auto target = build_joint_target(G1JointGroup::HEAD, head_single_joint, {0.2}, kJointTimeS);
      print_result("joint", robot.PublishTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_pose_target(0.2, 0.0, 0.0, kPoseTimeS);
      print_result("base_pose", robot.PublishTarget(target));
      continue;
    }

    if (command == "base_twist") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS);
      run_twist_scene(robot, "base_twist", target, kTwistDurationS);
      continue;
    }

    if (command == "mixed_pose") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G1JointGroup::HEAD, head_single_joint, {0.15}, kJointTimeS),
          build_chassis_pose_target(0.1, 0.0, 0.0, kPoseTimeS),
      });
      print_result("mixed_pose", robot.PublishTarget(target));
      continue;
    }

    if (command == "mixed_twist") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G1JointGroup::HEAD, head_single_joint, {-0.15}, kJointTimeS),
          build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS),
      });
      run_twist_scene(robot, "mixed_twist", target, kTwistDurationS);
      continue;
    }

    std::cout << "Unknown command: " << command << std::endl;
    print_menu();
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

直接请求原始目标（RequestTarget）

适用场景：高级用户直接构造 SingoriXTarget 并通过 request 通道下发到底层 WBCS，适合需要获取服务端错误返回的 raw target 场景。

examples/cpp/galbot_robot/src/request_target_example.cpp

/**
 * @file request_target_example.cpp
 * @brief G1 RequestTarget menu example for SDK mirror SingoriXTarget.
 *
 * This example shows how to construct `SingoriXTarget` directly at the SDK layer
 * and send it to the low-level WBCS through `RequestTarget()`.
 *
 * 1. Joint-space commands are written into `target_group_trajectory_map`
 *    - Typical for head / arm style joint-space control
 *    - Each group corresponds to one `TargetGroupTrajectory`
 *    - Each trajectory point is described by `GroupCommand + JointCommand`
 *
 * 2. Chassis pose / twist style task-space commands are written into
 *    `target_task_trajectory_map`
 *    - Typical for chassis pose / chassis twist control
 *    - Each task corresponds to one `TargetTaskTrajectory`
 *    - Each trajectory point is described by `TaskCommand + FrameTriad`
 *
 * 3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
 *    - This supports one-shot dispatch for whole-body / mixed control
 *
 * 4. The current SDK does not automatically switch the chassis controller when
 *    calling `PublishTarget()` / `RequestTarget()`
 *    - Therefore this example explicitly calls
 *      `switch_controller(G1ControllerName::CHASSIS_POSE_CTRL)` or
 *      `switch_controller(G1ControllerName::CHASSIS_TWIST_CTRL)` before
 *      chassis pose / twist / mixed scenes
 *
 * 5. For the base twist scene, this example automatically sends a zero-twist
 *    target after the configured duration to stop the chassis.
 */

#include <chrono>
#include <cstdint>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr const char* kChassisTaskName = "chassis";
constexpr const char* kChassisSubtaskPose = "chassis_pose";
constexpr const char* kChassisSubtaskTwist = "chassis_twist";

int64_t now_ns() {
  return std::chrono::duration_cast<std::chrono::nanoseconds>(
             std::chrono::system_clock::now().time_since_epoch())
      .count();
}

Quaternion yaw_to_quaternion(double yaw) {
  Quaternion q;
  q.z = std::sin(yaw * 0.5);
  q.w = std::cos(yaw * 0.5);
  return q;
}

SingoriXTarget make_empty_target() {
  SingoriXTarget target;
  target.header.timestamp_ns = now_ns();
  target.header.frame_id = "base_link";
  return target;
}

TargetConfig make_group_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_DEFAULT;
  config.target_type = TARGET_TYPE_PROVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DEFAULT;
  config.target_priority = 1;
  return config;
}

TargetConfig make_pose_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_POSE;
  config.target_type = TARGET_TYPE_OVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_LINEAR_INTERPOLATE;
  config.target_priority = 1;
  return config;
}

TargetConfig make_twist_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_TWIST;
  config.target_type = TARGET_TYPE_OVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DIRECT_PASS;
  config.target_priority = 1;
  return config;
}

SingoriXTarget build_joint_target(const std::string& group_name,
                                  const std::vector<std::string>& joint_names,
                                  const std::vector<double>& positions,
                                  double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& group_traj = target.target_group_trajectory_map[group_name];
  group_traj.target_config = make_group_target_config();
  group_traj.joint_names = joint_names;

  GroupCommand command;
  command.time_from_start_s = time_from_start_s;
  command.joint_commands.resize(joint_names.size());
  for (size_t i = 0; i < joint_names.size(); ++i) {
    command.joint_commands[i].position = positions[i];
    command.joint_commands[i].velocity = 0.0;
    command.joint_commands[i].acceleration = 0.0;
    command.joint_commands[i].effort = 0.0;
  }
  group_traj.group_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_pose_target(double x,
                                         double y,
                                         double yaw,
                                         double time_from_start_s,
                                         const std::string& frame_id = "rel(0)",
                                         const std::string& reference_frame_id = "odom") {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_pose_target_config();
  task_traj.group_names = {G1JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskPose) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = frame_id;
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = reference_frame_id;
  triad.pose = Pose();
  triad.pose->position.x = x;
  triad.pose->position.y = y;
  triad.pose->position.z = 0.0;
  triad.pose->orientation = yaw_to_quaternion(yaw);
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_twist_target(double vx,
                                          double vy,
                                          double wz,
                                          double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_twist_target_config();
  task_traj.group_names = {G1JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskTwist) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = "base_link";
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = "base_link";
  triad.twist = Twist();
  triad.twist->linear = Vector3{vx, vy, 0.0};
  triad.twist->angular = Vector3{0.0, 0.0, wz};
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_stop_twist_target() {
  return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1);
}

SingoriXTarget merge_targets(const std::vector<SingoriXTarget>& targets) {
  SingoriXTarget merged = make_empty_target();
  for (const auto& target : targets) {
    for (const auto& [group_name, group_traj] : target.target_group_trajectory_map) {
      merged.target_group_trajectory_map[group_name] = group_traj;
    }
    for (const auto& [task_name, task_traj] : target.target_task_trajectory_map) {
      merged.target_task_trajectory_map[task_name] = task_traj;
    }
  }
  return merged;
}

void print_menu() {
  std::cout << "\nAvailable commands:\n"
            << "  joint        - request a joint-only head target\n"
            << "  base_pose    - request a chassis pose target\n"
            << "  base_twist   - request a chassis twist target with auto stop\n"
            << "  mixed_pose   - request head + left_arm + chassis pose in one target\n"
            << "  mixed_twist  - request head + left_arm + chassis twist in one target\n"
            << "  quit         - exit example\n"
            << std::endl;
}

void print_error_info(const std::string& scene_name, const std::shared_ptr<ErrorInfo>& error_info) {
  if (error_info == nullptr) {
    std::cout << scene_name << ": RequestTarget returned nullptr" << std::endl;
    return;
  }
  if (error_info->error_vec.empty()) {
    std::cout << scene_name << ": RequestTarget success, service returned no errors" << std::endl;
    return;
  }

  std::cout << scene_name << ": RequestTarget returned " << error_info->error_vec.size() << " error entries:"
            << std::endl;
  for (const auto& error : error_info->error_vec) {
    std::cout << "  component=" << error.commpent << ", code=" << error.error_code
              << ", description=" << error.description << std::endl;
  }
}

ControlStatus ensure_controller(GalbotRobot& robot, const std::string& controller_name) {
  const auto status = robot.switch_controller(controller_name);
  std::cout << "switch_controller(" << controller_name << "): ";
  std::cout << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << std::endl;
  return status;
}

void run_twist_scene(GalbotRobot& robot,
                     const std::string& scene_name,
                     const SingoriXTarget& target,
                     double twist_duration_s) {
  std::cout << scene_name << ": start moving for " << twist_duration_s << " seconds" << std::endl;
  print_error_info(scene_name, robot.RequestTarget(target));
  std::this_thread::sleep_for(std::chrono::duration<double>(twist_duration_s));
  std::cout << scene_name << ": send stop twist target" << std::endl;
  print_error_info(scene_name + "_stop", robot.RequestTarget(build_stop_twist_target()));
}

}  // namespace

int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::seconds(2));

  const auto head_joint_names = robot.get_joint_names(true, {G1JointGroup::HEAD});
  if (head_joint_names.empty()) {
    std::cerr << "failed to fetch active head joints" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const std::vector<std::string> head_single_joint = {head_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;

  print_menu();

  std::string command;
  while (true) {
    std::cout << "Enter command: ";
    if (!(std::cin >> command)) {
      break;
    }
    if (command == "quit") {
      break;
    }

    if (command == "joint") {
      const auto target = build_joint_target(G1JointGroup::HEAD, head_single_joint, {0.2}, kJointTimeS);
      print_error_info("joint", robot.RequestTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_pose_target(0.2, 0.0, 0.0, kPoseTimeS);
      print_error_info("base_pose", robot.RequestTarget(target));
      continue;
    }

    if (command == "base_twist") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS);
      run_twist_scene(robot, "base_twist", target, kTwistDurationS);
      continue;
    }

    if (command == "mixed_pose") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G1JointGroup::HEAD, head_single_joint, {0.15}, kJointTimeS),
          build_chassis_pose_target(0.1, 0.0, 0.0, kPoseTimeS),
      });
      print_error_info("mixed_pose", robot.RequestTarget(target));
      continue;
    }

    if (command == "mixed_twist") {
      if (ensure_controller(robot, G1ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G1JointGroup::HEAD, head_single_joint, {-0.15}, kJointTimeS),
          build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS),
      });
      run_twist_scene(robot, "mixed_twist", target, kTwistDurationS);
      continue;
    }

    std::cout << "Unknown command: " << command << std::endl;
    print_menu();
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

停止底盘运动（stop_base）

适用场景：立即停止底盘的所有运动，紧急停止时使用。

examples/cpp/galbot_robot/src/stop_base_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Stop chassis motion
    while (true) {
        ControlStatus status = robot.stop_base();
        if (status == ControlStatus::SUCCESS) {
            std::cout << "Chassis motion has been stopped successfully!" << std::endl;
            break;
        } else {
            std::cerr << "Chassis stop motion failed, retrying..." << std::endl;
        }
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取并设置底盘速度（get_base_velocity && set_base_velocity）

duration_s=0 只发送一次；正值会阻塞调用线程，从首次成功发送后以 10Hz 续发指定时长，到期只停止发送，不调用 stop_base()。负数、非有限值及不可表示的时长返回 INVALID_INPUT。实际停止取决于底层看门狗，SUCCESS 不代表底盘已停止。不要在续发期间并发调用其他底盘命令，它们不会取消续发；需要自行控制停止时使用单次发送模式。

适用场景：控制和获取移动底盘的线速度和角速度，用于导航或远程遥控。

examples/cpp/galbot_robot/src/get_set_base_velocity_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_base_velocity(const std::shared_ptr<BaseVelocityInfo>& base_velocity_info) {
    if (!base_velocity_info) {
        std::cerr << "Base velocity data is empty" << std::endl;
        return;
    }

    // Print linear and angular velocity in the same format as the standalone example.
    const auto& linear = base_velocity_info->linear_velocity;
    std::cout << "Linear velocity (m/s): "
              << "vx=" << linear[0] << ", vy=" << linear[1] << ", vz=" << linear[2] << std::endl;

    const auto& angular = base_velocity_info->angular_velocity;
    std::cout << "Angular velocity (rad/s): "
              << "wx=" << angular[0] << ", wy=" << angular[1] << ", wz=" << angular[2] << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Read base velocity before issuing the motion command.
    auto base_velocity_before = robot.get_base_velocity();
    if (base_velocity_before) {
        std::cout << "Base velocity before command:" << std::endl;
        print_base_velocity(base_velocity_before);
    } else {
        std::cerr << "Failed to get base velocity before command." << std::endl;
    }

    // Please confirm the surrounding environment before chassis testing
    // Set chassis speed, linear_velocity first two fields are x and y velocities, angular_velocity third field is z rotation speed
    std::array<double, 3> linear_velocity = {0.2, 0.0, 0.0};    // 0.2 m/s
    std::array<double, 3> angular_velocity = {0.0, 0.0, 0.0};    // 0.0 rad/s
    double duration_s = 3.0;  // Block while publishing at 10 Hz; no stop is sent on expiry.

    if (robot.set_base_velocity(linear_velocity, angular_velocity, duration_s) == ControlStatus::SUCCESS) {
        std::cout << "Velocity publishing completed after " << duration_s
                  << " seconds; stopping depends on the watchdog." << std::endl;
    } else {
        std::cerr << "Set chassis speed failed." << std::endl;
    }

    // Observe immediately: publishing completion does not imply a stopped base.
    auto base_velocity_after = robot.get_base_velocity();
    if (base_velocity_after) {
        std::cout << "Base velocity after command:" << std::endl;
        print_base_velocity(base_velocity_after);
    } else {
        std::cerr << "Failed to get base velocity after command." << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取关节状态（get_joint_states）

适用场景：获取所有关节的完整状态信息，包括位置、速度、电流等。适用于需要完整关节信息的算法（如动力学计算、状态估计）。以下示例以左臂为例。

examples/cpp/galbot_robot/src/get_joint_states_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_joint_states(const std::vector<JointState>& joint_states) {
    for (const auto& states : joint_states) {
        std::cout << "--- Joint State ---" << std::endl;
        std::cout << "Joint Name:   " << states.joint_name   << std::endl;
        std::cout << "Position:     " << states.position     << " rad" << std::endl;
        std::cout << "Velocity:     " << states.velocity     << " rad/s" << std::endl;
        std::cout << "Acceleration: " << states.acceleration << " rad/s^2" << std::endl;
        std::cout << "Effort:       " << states.effort       << " Nm" << std::endl;
        std::cout << "Current:      " << states.current      << " A" << std::endl;
        std::cout << "Timestamp:    " << states.timestamp_ns << " ns" << std::endl;
        std::cout << "------------------" << std::endl;
    }
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get joint states by joint group names; returns all joints if empty
    std::vector<std::string> joint_groups = {"left_arm"};
    auto ret_states = robot.get_joint_states(joint_groups, {});
    if (ret_states.empty()) {
        std::cout << "No joint states returned for the specified joint groups" << std::endl;
    } else {
        print_joint_states(ret_states);
    }

    // Get specified joint states; if provided, overrides joint group input
    std::vector<std::string> joint_names = {"left_arm_joint1", "left_arm_joint2"};
    ret_states = robot.get_joint_states(joint_groups, joint_names);
    if (ret_states.empty()) {
        std::cout << "No joint states returned for the specified joint names" << std::endl;
    } else {
        print_joint_states(ret_states);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取关节位置（get_joint_positions）

适用场景：仅获取当前关节位置。当你只需要关节位置信息时使用此接口，比 get_joint_states 更轻量。

examples/cpp/galbot_robot/src/get_joint_positions_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_joint_positions(const std::vector<double>& positions) {
    for (const auto& pos : positions) {
        std::cout << "joint positions is " << pos << std::endl;
    }
    std::cout << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get joint positions by joint group names; returns all joints if empty
    std::vector<std::string> joint_groups = {"left_arm"};
    auto ret_positions = robot.get_joint_positions(joint_groups, {});
    std::cout << "Left arm joint positions:" << std::endl;
    print_joint_positions(ret_positions);

    // Get specified joint positions; if provided, overrides joint group input
    std::vector<std::string> joint_names = {"left_arm_joint1", "left_arm_joint2"};
    ret_positions = robot.get_joint_positions({joint_groups}, joint_names);
    std::cout << "Positions of left arm joint1 and joint2:" << std::endl;
    print_joint_positions(ret_positions);

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取关节名称（get_joint_names）

适用场景：获取机器人所有关节的名称列表，用于迭代遍历关节或配置文件生成。

examples/cpp/galbot_robot/src/get_joint_names_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get specified joint names; joint groups include ["leg", "head", "left_arm", "right_arm"]
    std::vector<std::string> joint_groups = {"head"};
    bool only_active_joint = true;  // Get active joints
    auto head_joint_names_vec =
        robot.get_joint_names(only_active_joint, joint_groups);
    std::cout << "Head joint names:" << std::endl;
    for (size_t i = 0; i < head_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << head_joint_names_vec[i] << std::endl;
    }

    // Passing an empty array returns all joint group information by default
    std::vector<std::string> null_vec = {};
    auto all_joint_names_vec =
        robot.get_joint_names(only_active_joint, null_vec);
    std::cout << "All joint names:" << std::endl;
    for (size_t i = 0; i < all_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << all_joint_names_vec[i] << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取夹爪状态（get_gripper_state）

适用场景：获取夹爪当前位置和状态，判断夹爪当前是打开还是闭合。

examples/cpp/galbot_robot/src/get_gripper_state_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_gripper_state(std::shared_ptr<GripperState> gripper_state) {
    std::cout << "Timestamp (ns): " << gripper_state->timestamp_ns << std::endl;

    std::cout << " width "  << gripper_state->width << " velocity " << gripper_state->velocity
                << " effort " << gripper_state->effort << " is moving "
                << gripper_state->is_moving << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get gripper state
    auto gripper_state_ptr = robot.get_gripper_state("left_gripper");

    if (gripper_state_ptr == nullptr) {
        std::cerr << "get gripper state error" << std::endl;
    } else {
        std::cout << "Left gripper state:" << std::endl;
        print_gripper_state(gripper_state_ptr);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取吸盘状态（get_suction_cup_state）

适用场景：获取吸盘当前是否处于吸合状态，以及是否成功检测到物体。

examples/cpp/galbot_robot/src/get_suction_cup_state_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_suction_cup_state(std::shared_ptr<SuctionCupState> suction_cup_state) {
    std::cout << "Timestamp (ns): " << suction_cup_state->timestamp_ns << std::endl;
    std::cout << "Activation: " << suction_cup_state->activation << std::endl;
    std::cout << "Pressure: " << suction_cup_state->pressure << " Pa" << std::endl;
    std::cout << "Action State: " << int(suction_cup_state->action_state) << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get suction cup state
    auto suction_cup_state_ptr = robot.get_suction_cup_state("right_suction_cup");

    if (suction_cup_state_ptr == nullptr) {
        std::cerr << "get suction cup state error" << std::endl;
    } else {
        std::cout << "Right suction cup status:" << std::endl;
        print_suction_cup_state(suction_cup_state_ptr);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取坐标变换（get_transform）

适用场景：获取两个坐标系之间的变换关系（位姿），用于机械臂抓取、视觉定位等场景。

examples/cpp/galbot_robot/src/get_transform_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_pose_vec(const std::vector<double> &pose_vec) {
    // Output pose_vec
    std::cout << "pose_vec = [";
    for (size_t i = 0; i < pose_vec.size(); ++i) {
        std::cout << pose_vec[i];
        if (i + 1 < pose_vec.size())
        std::cout << ", ";
    }
    std::cout << "]" << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get coordinate transform
    std::pair<std::vector<double>, int64_t> tf_ret = robot.get_transform("left_arm_link1", "left_arm_link7", 0);

    if (tf_ret.first.empty()) {
        std::cout << "get_transform error" << std::endl;
    } else {
        std::cout << "tf_timestamp_ns: " << tf_ret.second << std::endl;
        print_pose_vec(tf_ret.first);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取 IMU 数据（get_imu_data）

适用场景：获取机器人基座 IMU 的加速度、角速度和姿态信息，用于状态估计、运动分析。

examples/cpp/galbot_robot/src/get_imu_data_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_imu_data(const std::shared_ptr<ImuData>& imu_data) {
    if (!imu_data) {
        std::cerr << "IMU data is empty" << std::endl;
        return;
    }

    std::cout << "Timestamp (ns): " << imu_data->timestamp_ns << std::endl;

    std::cout << "Accelerometer: "
              << "x=" << imu_data->accel.x << ", "
              << "y=" << imu_data->accel.y << ", "
              << "z=" << imu_data->accel.z << std::endl;

    std::cout << "Gyroscope: "
              << "x=" << imu_data->gyro.x << ", "
              << "y=" << imu_data->gyro.y << ", "
              << "z=" << imu_data->gyro.z << std::endl;

    std::cout << "Magnetometer: "
              << "x=" << imu_data->magnet.x << ", "
              << "y=" << imu_data->magnet.y << ", "
              << "z=" << imu_data->magnet.z << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    std::unordered_set<SensorType> sensor_types = {
        SensorType::CHASSIS_IMU
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get IMU data
    // - SensorType::TORSO_IMU: torso IMU
    // - SensorType::LIDAR_IMU: lidar IMU
    // - SensorType::CHASSIS_IMU: Chassis lidar IMU
    std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::CHASSIS_IMU);
    if (imu_data) {
        std::cout << "IMU data retrieved successfully!" << std::endl;
        print_imu_data(imu_data);
    } else {
        std::cerr << "Failed to get IMU data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取电池信息（get_bms_information）

适用场景：获取电池电压、电量等信息，用于低电量检测和状态监控。

examples/cpp/galbot_robot/src/get_bms_information_example.cpp

#include <iostream>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_bms_information(const std::shared_ptr<BmsInfo>& bms_info) {
    if (!bms_info) {
        std::cerr << "BMS info is empty" << std::endl;
        return;
    }

    std::cout << "Voltage (V): " << bms_info->voltage << std::endl;
    std::cout << "Current (A): " << bms_info->current << std::endl;
    std::cout << "Battery level (%): " << bms_info->battery_level << std::endl;
    std::cout << "Temperature (C): " << bms_info->temperature << std::endl;
    std::cout << "Charging status: " << std::boolalpha << bms_info->charging_status
              << std::noboolalpha << std::endl;
    std::cout << "Health status: " << std::boolalpha << bms_info->health_status
              << std::noboolalpha << std::endl;
    std::cout << "Capacity (Ah): " << bms_info->capacity << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    // Get BMS information
    auto bms_info = robot.get_bms_information();
    if (bms_info) {
        std::cout << "BMS info retrieved successfully!" << std::endl;
        print_bms_information(bms_info);
    } else {
        std::cerr << "Failed to get BMS info!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取设备信息（get_device_information）

适用场景：获取机器人硬件版本、固件版本等设备信息，用于调试和兼容性检查。

examples/cpp/galbot_robot/src/get_device_information_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_device_info(const std::shared_ptr<DeviceInfo>& device_info) {
  if (!device_info) {
    std::cerr << "Device information is empty" << std::endl;
    return;
  }

  std::cout << "Device information:" << std::endl;
  std::cout << "  Model: " << (device_info->model.empty() ? "N/A" : device_info->model) << std::endl;
  std::cout << "  Serial number: " << (device_info->serial_number.empty() ? "N/A" : device_info->serial_number) << std::endl;
  std::cout << "  Firmware version: " << (device_info->firmware_version.empty() ? "N/A" : device_info->firmware_version)
            << std::endl;
  std::cout << "  Hardware version: " << (device_info->hardware_version.empty() ? "N/A" : device_info->hardware_version)
            << std::endl;
  std::cout << "  : " << (device_info->manufacturer.empty() ? "N/A" : device_info->manufacturer) << std::endl;
}

int main() {
  // Get object instance
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize system
  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // Get device information
  std::shared_ptr<DeviceInfo> device_info = robot.get_device_information();
  if (device_info) {
    std::cout << "Device information retrieved successfully!" << std::endl;
    print_device_info(device_info);
  } else {
    std::cerr << "Failed to get device information!" << std::endl;
  }

  // Exit system and release SDK resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

获取相机图像数据（get_rgb_data && get_depth_data）(get_camera_data)

适用场景：获取 RGB 图像和深度图像数据，用于视觉感知、目标检测、SLAM 等任务。

examples/cpp/galbot_robot/src/get_camera_data_example.cpp

#include <iostream>
#include <vector>
#include <memory>
#include <unordered_set>
#include <chrono>
#include <thread>

#include "galbot_robot.hpp"
#include "opencv2/opencv.hpp"

using namespace galbot::sdk;

void print_rgb_data(const std::shared_ptr<RgbData> &rgb_data) {
  if (rgb_data == nullptr) {
    std::cout << "rgb_data is nullptr" << std::endl;
    return;
  }

  std::cout << "Camera image timestamp: "
            << rgb_data->header.timestamp_ns
            << std::endl;
  std::cout << "format is " << rgb_data->format << std::endl;
  std::cout << "frame_id is " << rgb_data->header.frame_id << std::endl;
  std::cout << "data size is " << rgb_data->data.size() << std::endl;

  std::cout << "show image:";

  std::shared_ptr<cv::Mat> img = rgb_data->convert_to_cv2_mat();

  cv::imwrite("result_image.jpg", *img);

  std::cout << "Image saved to result_image.jpg" << std::endl;
}

void print_ir_data(const std::string& name, const std::shared_ptr<IrData>& ir_data) {
  if (ir_data == nullptr) {
    std::cout << name << " is nullptr" << std::endl;
    return;
  }

  std::cout << name << " timestamp: " << ir_data->header.timestamp_ns << std::endl;
  std::cout << "format is " << ir_data->format << std::endl;
  std::cout << "frame_id is " << ir_data->header.frame_id << std::endl;
  std::cout << "size is " << ir_data->width << "x" << ir_data->height << std::endl;
  std::cout << "data size is " << ir_data->data.size() << std::endl;

  std::shared_ptr<cv::Mat> img = ir_data->convert_to_cv2_mat();
  if (img && !img->empty()) {
    std::string filename = name + ".png";
    cv::imwrite(filename, *img);
    std::cout << "Image saved to " << filename << std::endl;
  }
}

void print_depth_data(const std::shared_ptr<DepthData> depth_data_ptr) {
  if (depth_data_ptr == nullptr) {
    std::cout << "depth_data_ptr is nullptr" << std::endl;
    return;
  }

  std::cout << "Camera image timestamp: "
            << depth_data_ptr->header.timestamp_ns
            << std::endl;
  std::cout << "format is " << depth_data_ptr->format << std::endl;
  std::cout << "frame_id is " << depth_data_ptr->header.frame_id << std::endl;
  std::cout << "data size is " << depth_data_ptr->data.size() << std::endl;

  std::shared_ptr<cv::Mat> img = depth_data_ptr->convert_to_cv2_mat();

  if (img && !img->empty()) {
    cv::Mat img_vis;

    // Image information normalization
    cv::normalize(*img, img_vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);

    // Pseudo-color enhancement
    cv::Mat img_color;
    cv::applyColorMap(img_vis, img_color, cv::COLORMAP_JET);

    // save
    cv::imwrite("check_raw_data.png", *img); 
    cv::imwrite("check_visual_view.jpg", img_color); 

    std::cout << "Image saved: \n"
              << "1. check_raw_data.png -> A fully black image containing real physical depth\n"
              << "2. check_visual_view.jpg -> A colorized image where object contours are visible" << std::endl;
    }
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_LEFT_CAMERA,           // Head left camera
        SensorType::LEFT_ARM_DEPTH_CAMERA,      // Left arm depth camera
        SensorType::LEFT_ARM_INFRA_CAMERA_1,    // Left arm IR camera 1
        SensorType::LEFT_ARM_INFRA_CAMERA_2,    // Left arm IR camera 2
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }
    // Wait for camera data ready
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get RGB image data
    std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::HEAD_LEFT_CAMERA, RgbOutputFormat::JPEG, true);
    if (rgb_data) {
        std::cout << "RGB image data retrieved successfully!" << std::endl;
        print_rgb_data(rgb_data);
    } else {
        std::cerr << "Failed to get RGB image data!" << std::endl;
    }

    // Get depth image data
    std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
    if (depth_data) {
        std::cout << "Depth image data retrieved successfully!" << std::endl;
        print_depth_data(depth_data);
    } else {
        std::cerr << "Failed to get depth image data!" << std::endl;
    }

    // Get left arm IR camera 1 image data
    std::shared_ptr<IrData> ir1_data = robot.get_ir_data(SensorType::LEFT_ARM_INFRA_CAMERA_1);
    if (ir1_data) {
        std::cout << "IR camera 1 data retrieved successfully!" << std::endl;
        print_ir_data("left_arm_infra1", ir1_data);
    } else {
        std::cerr << "Failed to get IR camera 1 data!" << std::endl;
    }

    // Get left arm IR camera 2 image data
    std::shared_ptr<IrData> ir2_data = robot.get_ir_data(SensorType::LEFT_ARM_INFRA_CAMERA_2);
    if (ir2_data) {
        std::cout << "IR camera 2 data retrieved successfully!" << std::endl;
        print_ir_data("left_arm_infra2", ir2_data);
    } else {
        std::cerr << "Failed to get IR camera 2 data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

订阅视频数据（subscribe_video_data）

适用场景：订阅指定传感器的 H.264 视频流并通过回调接收编码帧，适用于录像、实时处理、网络转发等需要基于推送的压缩视频交付场景。

examples/cpp/galbot_robot/src/subscribe_video_data_example.cpp

#include <chrono>
#include <fstream>
#include <iostream>
#include <memory>
#include <mutex>
#include <thread>
#include <unordered_set>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {
constexpr const char* kOutputPath = "head_left.h264";
constexpr int kDurationSeconds = 10;
constexpr int kLogEveryNFrames = 30;

std::mutex g_state_mutex;
std::ofstream g_output_file;
int g_frame_count = 0;

// Important:
// The video callback is executed by the SDK internal dispatch thread.
// Do not perform time-consuming operations in this callback; keep it
// short and non-blocking. Long-running work may delay video frame delivery
// for this subscription.
void video_data_callback(const std::shared_ptr<EncodedVideoData>& video_data) {
  if (video_data == nullptr) {
    return;
  }

  std::lock_guard<std::mutex> lock(g_state_mutex);
  if (g_output_file.is_open() && !video_data->data.empty()) {
    g_output_file.write(reinterpret_cast<const char*>(video_data->data.data()),
                        static_cast<std::streamsize>(video_data->data.size()));
  }

  ++g_frame_count;
  if (g_frame_count == 1 || g_frame_count % kLogEveryNFrames == 0) {
    std::cout << "frame_count=" << g_frame_count
              << ", timestamp_ns=" << video_data->header.timestamp_ns
              << ", format=" << video_data->format
              << ", bytes=" << video_data->data.size() << std::endl;
  }
}
}  // namespace

int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  std::unordered_set<SensorType> sensor_types = {SensorType::HEAD_LEFT_CAMERA};
  if (!robot.init(sensor_types)) {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }
  std::cout << "System initialized successfully!" << std::endl;

  g_output_file.open(kOutputPath, std::ios::binary | std::ios::trunc);
  if (!g_output_file.is_open()) {
    std::cerr << "Failed to open output file: " << kOutputPath << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }
  std::cout << "writing H.264 stream to " << kOutputPath << std::endl;

  SensorStatus status = robot.subscribe_video_data(SensorType::HEAD_LEFT_CAMERA, video_data_callback);
  if (status != SensorStatus::SUCCESS) {
    std::cerr << "subscribe_video_data failed, status=" << static_cast<int>(status) << std::endl;
    g_output_file.close();
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }
  std::cout << "subscribe_video_data success" << std::endl;

  std::this_thread::sleep_for(std::chrono::seconds(kDurationSeconds));

  SensorStatus unsubscribe_status = robot.unsubscribe_video_data(SensorType::HEAD_LEFT_CAMERA);
  std::cout << "unsubscribe_video_data status=" << static_cast<int>(unsubscribe_status) << std::endl;

  {
    std::lock_guard<std::mutex> lock(g_state_mutex);
    if (g_output_file.is_open()) {
      g_output_file.close();
    }
    std::cout << "received " << g_frame_count << " frames" << std::endl;
  }
  std::cout << "saved H.264 stream to " << kOutputPath << std::endl;

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released successfully" << std::endl;
  return 0;
}

获取传感器内外参（get_camera_intrinsic && get_sensor_extrinsic）

适用场景：获取相机内参和传感器相对于机器人基座的外参，用于计算机视觉计算和 3D 重建。

examples/cpp/galbot_robot/src/get_camera_params.cpp

#include <iostream>
#include <vector>
#include <memory>
#include <unordered_set>
#include <chrono>
#include <thread>

#include "galbot_robot.hpp"
#include "opencv2/opencv.hpp"

using namespace galbot::sdk;

void print_rgb_data(
  const std::shared_ptr<RgbData> &rgb_data) {
  if (rgb_data == nullptr) {
    std::cout << "rgb_data is nullptr" << std::endl;
    return;
  }

  std::cout << "Camera image timestamp: "
            << rgb_data->header.timestamp_ns
            << std::endl;
  std::cout << "format is " << rgb_data->format << std::endl;
  std::cout << "frame_id is " << rgb_data->header.frame_id << std::endl;
  std::cout << "data size is " << rgb_data->data.size() << std::endl;

  std::cout << "show image:";

  std::shared_ptr<cv::Mat> img = rgb_data->convert_to_cv2_mat();

  cv::imwrite("result_image.jpg", *img);

  std::cout << "Image saved to result_image.jpg" << std::endl;
}

void print_depth_data(
    const std::shared_ptr<DepthData>
        depth_data_ptr) {
  if (depth_data_ptr == nullptr) {
    std::cout << "depth_data_ptr is nullptr" << std::endl;
    return;
  }

  std::cout << "Camera image timestamp: "
            << depth_data_ptr->header.timestamp_ns
            << std::endl;
  std::cout << "format is " << depth_data_ptr->format << std::endl;
  std::cout << "frame_id is " << depth_data_ptr->header.frame_id << std::endl;
  std::cout << "data size is " << depth_data_ptr->data.size() << std::endl;

  std::shared_ptr<cv::Mat> img = depth_data_ptr->convert_to_cv2_mat();

  if (img && !img->empty()) {
    cv::Mat img_vis;

    // Image information normalization
    cv::normalize(*img, img_vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);

    // Pseudo-color enhancement
    cv::Mat img_color;
    cv::applyColorMap(img_vis, img_color, cv::COLORMAP_JET);

    // save
    cv::imwrite("check_raw_data.png", *img); 
    cv::imwrite("check_visual_view.jpg", img_color); 

    std::cout << "Image saved: \n"
              << "1. check_raw_data.png -> A fully black image containing real physical depth\n"
              << "2. check_visual_view.jpg -> A colorized image where object contours are visible" << std::endl;
    }
}

void print_camera_info(const std::shared_ptr<CameraInfo>& camerainfo){
    if (camerainfo == nullptr) {
        std::cout << "camerainfo is nullptr" << std::endl;
        return;
    }

    std::cout << "camera info:" << std::endl;
    std::cout << "header {" << std::endl;
    std::cout << "  timestamp_ns: " << camerainfo->header.timestamp_ns << std::endl;
    std::cout << "  frame_id: " << camerainfo->header.frame_id << std::endl;
    std::cout << "}" << std::endl;
    std::cout << "width: " << camerainfo->width << std::endl;
    std::cout << "height: " << camerainfo->height << std::endl;
    std::cout << "distortion_model: " << camerainfo->distortion_model << std::endl;
    std::cout << "d: ";
    for (auto& d_i : camerainfo->d) {
        std::cout << d_i << " ";
    }
    std::cout << std::endl;
    std::cout << "k: ";
    for (auto& k_i : camerainfo->k) {
        std::cout << k_i << " ";
    }
    std::cout << std::endl;
    std::cout << "r: ";
    for (auto& r_i : camerainfo->r) {
        std::cout << r_i << " ";
    }
    std::cout << std::endl;
    std::cout << "p: ";
    for (auto& p_i : camerainfo->p) {
        std::cout << p_i << " ";
    }
    std::cout << std::endl;
    std::cout << "T: ";
    for (auto& t_i : camerainfo->T) {
        std::cout << t_i << " ";
    }
    std::cout << std::endl;
    std::cout << "roi {" << std::endl;
    std::cout << "  width: " << camerainfo->roi.width << std::endl;
    std::cout << "  height: " << camerainfo->roi.height << std::endl;
    std::cout << "}" << std::endl;
    std::cout << "camera_type: " << camerainfo->camera_type << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_LEFT_CAMERA,       // Head left camera
        SensorType::LEFT_ARM_DEPTH_CAMERA,  // Left arm depth camera
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }
    // Wait for camera data ready
    std::this_thread::sleep_for(std::chrono::milliseconds(10000));

    // Get RGB image data
    std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::HEAD_LEFT_CAMERA, RgbOutputFormat::JPEG, true);
    if (rgb_data) {
        std::cout << "RGB image data retrieved successfully!" << std::endl;
        print_rgb_data(rgb_data);
    } else {
        std::cerr << "Failed to get RGB image data!" << std::endl;
    }

    // Get camera parameters
    std::shared_ptr<CameraInfo> rgb_camerainfo = robot.get_camera_intrinsic(SensorType::LEFT_ARM_DEPTH_CAMERA);
    if (rgb_camerainfo) {
        std::cout << "Camera parameters retrieved successfully!" << std::endl;
        print_camera_info(rgb_camerainfo);
    } else {
        std::cerr << "Failed to get camera parameters!" << std::endl;
    }

    // Get depth image data
    std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
    if (depth_data) {
        std::cout << "Depth image data retrieved successfully!" << std::endl;
        print_depth_data(depth_data);
    } else {
        std::cerr << "Failed to get depth image data!" << std::endl;
    }

    // Get sensor extrinsics
    std::pair<std::vector<double>, int64_t> sensor_extrinsic = robot.get_sensor_extrinsic(SensorType::LEFT_ARM_DEPTH_CAMERA);
    if (!sensor_extrinsic.first.empty()) {
        std::cout << "Sensor extrinsics retrieved successfully!" << std::endl;
        std::cout << "transform: ";
        for (auto& t_i : sensor_extrinsic.first) {
            std::cout << t_i << " ";
        }
        std::cout << std::endl;
        std::cout << "timestamp: " << sensor_extrinsic.second << std::endl;
    } else {
        std::cerr << "Failed to get sensor extrinsics!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取雷达数据（get_lidar_data）

适用场景：获取激光雷达点云数据，用于导航、避障、环境建模。

examples/cpp/galbot_robot/src/get_lidar_data_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <cmath>
#include <chrono>
#include <thread>
#include <memory>
#include <unordered_set>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Define point structure
struct Point3D {
    float x, y, z;
};

/**
 * Directly extract the XYZ array from LidarData via pointer operations
 */
std::vector<Point3D> get_xyz_points(const std::shared_ptr<LidarData>& cloud, bool remove_nan = false) {
    std::vector<Point3D> points;
    if (!cloud || cloud->data.empty()) return points;

    // 1. Find the offsets of x, y, z fields (offset)
    // In the Python version, fields are indexed by field name; here we precompute offsets for better performance
    int32_t off_x = -1, off_y = -1, off_z = -1;
    for (const auto& f : cloud->fields) {
        if (f.name == "x") off_x = f.offset;
        else if (f.name == "y") off_y = f.offset;
        else if (f.name == "z") off_z = f.offset;
    }

    if (off_x == -1 || off_y == -1 || off_z == -1) {
        std::cerr << "Error: point cloud data is missing required xyz fields" << std::endl;
        return points;
    }

    uint32_t num_points = cloud->width * cloud->height;
    points.reserve(num_points);

    const uint8_t* raw_data = cloud->data.data();
    uint32_t point_step = cloud->point_step;

    // 2. Read directly via pointer (core zero-copy logic)
    for (uint32_t i = 0; i < num_points; ++i) {
        // Calculate starting pointer for current point
        const uint8_t* pt_ptr = raw_data + (i * point_step);

        // Use reinterpret_cast to directly cast pointer types and read memory
        // Assume LiDAR data is float32 (F), which is the most common format
        float x = *reinterpret_cast<const float*>(pt_ptr + off_x);
        float y = *reinterpret_cast<const float*>(pt_ptr + off_y);
        float z = *reinterpret_cast<const float*>(pt_ptr + off_z);

        // Handle NaN values (corresponds to Python remove_nan logic)
        if (remove_nan) {
            if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
                continue;
            }
        }

        points.push_back({x, y, z});
    }

    return points;
}

/**
 * Save to a PCD file
 */
void save_xyz_to_pcd(const std::vector<Point3D>& points, const std::string& filename) {
    std::ofstream fs(filename);
    if (!fs.is_open()) {
        std::cerr << "Unable to open file for writing: " << filename << std::endl;
        return;
    }

    // PCD 0.7 Header
    fs << "# .PCD v0.7 - Point Cloud Data file format\n"
       << "VERSION 0.7\n"
       << "FIELDS x y z\n"
       << "SIZE 4 4 4\n"
       << "TYPE F F F\n"
       << "COUNT 1 1 1\n"
       << "WIDTH " << points.size() << "\n"
       << "HEIGHT 1\n"
       << "VIEWPOINT 0 0 0 1 0 0 0\n"
       << "POINTS " << points.size() << "\n"
       << "DATA ascii\n";

    // Data
    for (const auto& p : points) {
        fs << p.x << " " << p.y << " " << p.z << "\n";
    }

    fs.close();
    std::cout << "Saved " << points.size() << " points to " << filename << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::BASE_LIDAR              // Chassis lidar
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get LiDAR data
    std::shared_ptr<LidarData> lidar_data = robot.get_lidar_data(SensorType::BASE_LIDAR);
    if (lidar_data) {
        std::cout << "Lidar data retrieved successfully!" << std::endl;
        std::vector<Point3D> xyz_points = get_xyz_points(lidar_data, false);
        if (!xyz_points.empty()) {
            save_xyz_to_pcd(xyz_points, "output_xyz.pcd");
        }
    } else {
        std::cerr << "Failed to get lidar data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取同步观测数据（get_synced_observation）

适用场景：获取同步观测数据，主要用于模型推理场景下获取时序对齐的输入数据，支持彩色相机、深度相机和关节数据。

examples/cpp/galbot_robot/src/get_synced_observation.cpp

#include <algorithm>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_set>
#include <vector>

#include "galbot_robot.hpp"

namespace {

std::string sensor_name(galbot::sdk::SensorType sensor) {
  using galbot::sdk::SensorType;
  switch (sensor) {
    case SensorType::HEAD_LEFT_CAMERA:
      return "HEAD_LEFT_CAMERA";
    case SensorType::HEAD_RIGHT_CAMERA:
      return "HEAD_RIGHT_CAMERA";
    case SensorType::LEFT_ARM_CAMERA:
      return "LEFT_ARM_CAMERA";
    case SensorType::RIGHT_ARM_CAMERA:
      return "RIGHT_ARM_CAMERA";
    case SensorType::LEFT_ARM_DEPTH_CAMERA:
      return "LEFT_ARM_DEPTH_CAMERA";
    case SensorType::RIGHT_ARM_DEPTH_CAMERA:
      return "RIGHT_ARM_DEPTH_CAMERA";
    default:
      return "UNKNOWN_CAMERA";
  }
}

}  // namespace

int main() {
  using namespace galbot::sdk;

  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  std::unordered_set<SensorType> enable_sensor_set = {
      SensorType::HEAD_LEFT_CAMERA,
      SensorType::LEFT_ARM_CAMERA,
      SensorType::RIGHT_ARM_CAMERA,
  };
  if (!robot.init(enable_sensor_set, true)) {
    std::cout << "init failed" << std::endl;
    robot.destroy();
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(300));

  const std::vector<SensorType> cameras = {
      SensorType::LEFT_ARM_CAMERA,   // anchor camera
      SensorType::RIGHT_ARM_CAMERA,  // nearest-neighbor aligned
      SensorType::HEAD_LEFT_CAMERA,  // nearest-neighbor aligned
  };
  auto obs = robot.get_synced_observation(cameras, true);
  if (!obs) {
    std::cout << "get_synced_observation failed" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  // Sync-mode RGB payloads are tightly packed CPU-owned NV12, not JPEG.
  std::cout << "[Synced RGB]" << std::endl;
  const auto anchor_it = obs->rgb_data_map.find(cameras[0]);
  if (anchor_it == obs->rgb_data_map.end() || !anchor_it->second) {
    std::cout << "  anchor missing" << std::endl;
  } else {
    const int64_t anchor_ts_ns = anchor_it->second->header.timestamp_ns;
    std::cout << "  anchor=" << sensor_name(cameras[0]) << " timestamp_ns=" << anchor_ts_ns << std::endl;
    for (const auto& cam : cameras) {
      const auto it = obs->rgb_data_map.find(cam);
      if (it == obs->rgb_data_map.end() || !it->second) {
        std::cout << "  " << sensor_name(cam) << ": missing" << std::endl;
        continue;
      }
      const int64_t cam_ts_ns = it->second->header.timestamp_ns;
      std::cout << "  " << sensor_name(cam) << " timestamp_ns=" << cam_ts_ns
                << " delta_to_anchor_ms=" << (cam_ts_ns - anchor_ts_ns) / 1e6
                << " format=" << it->second->format
                << " dimensions=" << it->second->width << "x" << it->second->height
                << " bytes=" << it->second->data.size() << std::endl;
    }
  }

  if (obs->joint_state) {
    std::cout << "[Joint] timestamp_ns=" << obs->joint_state->timestamp_ns
              << " joint_count=" << obs->joint_state->joint_state_vec.size() << std::endl;
    const size_t print_n = std::min<size_t>(obs->joint_state->joint_state_vec.size(), 5);
    for (size_t i = 0; i < print_n; ++i) {
      const auto& js = obs->joint_state->joint_state_vec[i];
      std::cout << "  [" << i << "] joint_name=" << js.joint_name
                << " position=" << js.position
                << " velocity=" << js.velocity
                << " effort=" << js.effort << std::endl;
    }
  } else {
    std::cout << "[Joint] missing" << std::endl;
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

一键回零-odom坐标系（whole_body_reset_zero_odom）

适用场景：快速将全身上下所有关节运动到零位（初始姿态），基于 odom 坐标系。通常用于程序启动后的初始化。

examples/cpp/galbot_robot/src/whole_body_reset_zero_odom_example.cpp

#include <iostream>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {
const char* control_status_to_string(ControlStatus status) {
    switch (status) {
        case ControlStatus::SUCCESS:
            return "SUCCESS";
        case ControlStatus::TIMEOUT:
            return "TIMEOUT";
        case ControlStatus::FAULT:
            return "FAULT";
        case ControlStatus::INVALID_INPUT:
            return "INVALID_INPUT";
        case ControlStatus::INIT_FAILED:
            return "INIT_FAILED";
        case ControlStatus::IN_PROGRESS:
            return "IN_PROGRESS";
        case ControlStatus::STOPPED_UNREACHED:
            return "STOPPED_UNREACHED";
        case ControlStatus::DATA_FETCH_FAILED:
            return "DATA_FETCH_FAILED";
        case ControlStatus::PUBLISH_FAIL:
            return "PUBLISH_FAIL";
        case ControlStatus::COMM_DISCONNECTED:
            return "COMM_DISCONNECTED";
        default:
            return "UNKNOWN_STATUS";
    }
}
}  // namespace

int main() {
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    auto& motion = GalbotMotion::get_instance(MachineType::G1);

    if (!robot.init()) {
        std::cerr << "GalbotRobot init failed." << std::endl;
        return -1;
    }
    if (!motion.init()) {
        std::cerr << "GalbotMotion init failed." << std::endl;
        return -1;
    }

    // Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
    const std::string frame_id = "base_link";
    const std::string reference_frame_id = "odom";

    // reset to zero
    auto result = robot.zero_whole_body_and_base(
        frame_id,
        reference_frame_id,
        /*is_blocking=*/true,
        /*leg_head_speed_rad_s=*/0.2,
        /*leg_head_timeout_s=*/15.0,
        /*params=*/default_param);
    
    std::cout << "Zero joint status: " << motion.status_to_string(result.first) << std::endl;
    std::cout << "Zero base status: " << control_status_to_string(result.second) << std::endl;

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

一键回零-map坐标系（whole_body_reset_zero_map）

适用场景：快速将全身上下所有关节运动到零位（初始姿态），基于 map 坐标系。

examples/cpp/galbot_robot/src/whole_body_reset_zero_map_example.cpp

#include <iostream>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {
const char* control_status_to_string(ControlStatus status) {
    switch (status) {
        case ControlStatus::SUCCESS:
            return "SUCCESS";
        case ControlStatus::TIMEOUT:
            return "TIMEOUT";
        case ControlStatus::FAULT:
            return "FAULT";
        case ControlStatus::INVALID_INPUT:
            return "INVALID_INPUT";
        case ControlStatus::INIT_FAILED:
            return "INIT_FAILED";
        case ControlStatus::IN_PROGRESS:
            return "IN_PROGRESS";
        case ControlStatus::STOPPED_UNREACHED:
            return "STOPPED_UNREACHED";
        case ControlStatus::DATA_FETCH_FAILED:
            return "DATA_FETCH_FAILED";
        case ControlStatus::PUBLISH_FAIL:
            return "PUBLISH_FAIL";
        case ControlStatus::COMM_DISCONNECTED:
            return "COMM_DISCONNECTED";
        default:
            return "UNKNOWN_STATUS";
    }
}
}  // namespace

int main() {
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    auto& motion = GalbotMotion::get_instance(MachineType::G1);

    if (!robot.init()) {
        std::cerr << "GalbotRobot init failed." << std::endl;
        return -1;
    }
    if (!motion.init()) {
        std::cerr << "GalbotMotion init failed." << std::endl;
        return -1;
    }

    // Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
    const std::string frame_id = "base_link";
    const std::string reference_frame_id = "map";

    // reset to zero
    auto result = robot.zero_whole_body_and_base(
        frame_id,
        reference_frame_id,
        /*is_blocking=*/true,
        /*leg_head_speed_rad_s=*/0.2,
        /*leg_head_timeout_s=*/15.0,
        /*params=*/default_param);
    
    std::cout << "Zero joint status: " << motion.status_to_string(result.first) << std::endl;
    std::cout << "Zero base status: " << control_status_to_string(result.second) << std::endl;

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

紧急停止与恢复（emergency_stop / resume_from_emergency_stop）

适用场景：立即切断所有电机驱动，用于需要快速断电的安全场景；随后在触发紧急停止后重新使能所有电机，恢复机器人正常运行状态。

examples/cpp/galbot_robot/src/emergency_stop_and_resume_example.cpp

#include <iostream>
#include <chrono>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "Initialization succeeded" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    // Trigger software emergency stop
    ControlStatus status = robot.emergency_stop();
    if (status == ControlStatus::SUCCESS) {
        std::cout << "Emergency stop successfully." << std::endl;
    } else {
        std::cout << "Emergency stop failed." << std::endl;
    }

    std::cout << "Waiting 5 seconds before resuming from emergency stop..." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(5));

    // Resume from software emergency stop
    status = robot.resume_from_emergency_stop();
    if (status == ControlStatus::SUCCESS) {
        std::cout << "Resume from emergency stop successfully." << std::endl;
    } else {
        std::cout << "Resume from emergency stop failed." << std::endl;
    }

    std::cout << "Waiting 15 seconds before exiting..." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(15));

    // Send exit signal
    robot.request_shutdown();
    // Wait until entering shutdown state
    robot.wait_for_shutdown();
    // Release SDK resources
    robot.destroy();
    std::cout << "Resources released successfully" << std::endl;

    return 0;
}

音频播放（play_audio / stop_audio）

适用场景：通过机器人扬声器播放音频文件，并停止当前音频播放任务。

examples/cpp/galbot_robot/src/audio_example.cpp

#include <chrono>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <limits>
#include <sstream>
#include <string>
#include <unordered_set>
#include <vector>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Added command help table structure
struct CommandInfo {
  std::string cmd;
  std::string description;
  std::string parameters;
  std::string example;
};

// Command help table
const std::vector<CommandInfo> COMMAND_TABLE = {
    {"start_microphone_stream_input", "Start microphone streaming audio input feature", "No parameters", "start_microphone_stream_input"},
    {"stop_microphone_stream_input", "Stop the specified microphone streaming audio input", "No parameters", "stop_microphone_stream_input"},
    {"write_audio_stream_output", "PCM audiodata audio output", "Audio file name to play (16K 16bit Mono PCM audio file path)", "write_audio_stream_output"},
    {"stop_audio_stream_output", "Stop the specified audio output stream or all active audio output streams", "No parameters", "stop_audio_stream_output"},
    {"set_volume", "Set system global volume", "Volume setting parameter (0-100)", "set_volume"},
    {"get_volume", "Get current system global volume", "No parameters", "get_volume"},
    {"help", "Show help", "No parameters", "help"},
    {"q", "Exit program", "No parameters", "q"}};

// command
void printCommandHelp() {
  std::cout << "\n================ Command Help Table =================" << std::endl;
  std::cout << std::left << std::setw(30) << "Command" << std::setw(30) << "Description" << std::setw(30) << "Parameters"
            << std::setw(30) << "example" << std::endl;
  std::cout << std::string(110, '-') << std::endl;

  for (const auto& cmd : COMMAND_TABLE) {
    std::cout << std::left << std::setw(40) << cmd.cmd << std::setw(60) << cmd.description << std::setw(40)
              << cmd.parameters << std::setw(30) << cmd.example << std::endl;
  }
  std::cout << "=============================================\n" << std::endl;
}


void save_audio_to_file(const std::string& filename, const std::vector<uint8_t>& audio_data) {
  if (audio_data.empty()) {
    return;
  }
  std::ofstream outfile(filename, std::ios::app | std::ios::binary);
  if (!outfile) {
    std::cout << "Failed to open file for writing: " << filename << std::endl;
    return;
  }
  outfile.write(reinterpret_cast<const char*>(audio_data.data()),
                static_cast<std::streamsize>(audio_data.size()));
}

void handle_audio_data(const std::shared_ptr<AudioData>& audio_data) {
  if (audio_data == nullptr) {
    return;
  }

  const int64_t timestamp_ns = audio_data->header.timestamp_ns;
  (void)timestamp_ns;

  const std::string& audio_type = audio_data->type;
  const auto data_size = audio_data->data.size();
  (void)audio_data->format;

  if (audio_type == "vad_begin") {
    std::cout << "\n[VAD_BEGIN] Voice activity detection started" << std::endl;
  } else if (audio_type == "vad_end") {
    std::cout << "[VAD_END] Voice activity detection ended" << std::endl;
  } else if (audio_type == "vad_chunk") {
    // std::cout << "[AUDIO_CHUNK] Received " << data_size << " bytes of " << audio_format
    //           << " audio data (timestamp_ns: " << timestamp_ns << ")" << std::endl;
    save_audio_to_file("mic_vad.pcm", audio_data->data);
  } else if (audio_type == "denoise_chunk") {
    // std::cout << "[DENOISE_AUDIO] Received " << data_size << " bytes of denoised audio (timestamp_ns: "
    //           << timestamp_ns << ")" << std::endl;
    save_audio_to_file("mic_denoise.pcm", audio_data->data);
  } else if (audio_type == "waken_up") {
    std::string payload(audio_data->data.begin(), audio_data->data.end());
    std::cout << "\n[WAKEN_UP] Wake-up event triggered: " << payload << std::endl;
  } else {
    std::cout << "[" << audio_type << "] Received audio data, size: " << data_size << " bytes" << std::endl;
  }
}


// Safely read numeric inputs of multiple types and handle input errors
template<typename T>
bool safe_read_number(T& value, const std::string& prompt = "") {
  if (!prompt.empty()) {
    std::cout << prompt;
  }

  std::string input;
  std::getline(std::cin, input);

  // Check input stream status
  if (std::cin.fail() && !std::cin.eof()) {
    std::cin.clear();
    std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    std::cout << "Input error, please enter a valid number." << std::endl;
    return false;
  }

  // Trim leading and trailing spaces
  input.erase(0, input.find_first_not_of(" \t"));
  input.erase(input.find_last_not_of(" \t") + 1);

  if (input.empty()) {
    std::cout << "Input error, please enter a valid number." << std::endl;
    return false;
  }

  try {
    size_t pos;
    T result;

    if (std::is_integral<T>()) {
      result = static_cast<T>(std::stoll(input, &pos));
    } else if (std::is_floating_point<T>()) {
      result = static_cast<T>(std::stold(input, &pos));
    }

    // Check whether the entire string has been converted
    if (pos != input.length()) {
      std::cout << "Input error, please enter a valid number." << std::endl;
      return false;
    }

    value = result;
    return true;
  } catch (const std::exception&) {
    std::cout << "Input error, please enter a valid number." << std::endl;
    return false;
  }
}


// Safely read filename input and handle input errors
bool safe_read_file_name(std::string& file_name, const std::string& prompt = "") {
  if (!prompt.empty()) {
    std::cout << prompt;
  }

  std::getline(std::cin, file_name);

  // Check input stream status
  if (std::cin.fail() && !std::cin.eof()) {
    std::cin.clear();
    std::cin.ignore();
  }
  return true;
}

int main(int argc, char* argv[]) {
  std::cout << "start" << std::endl;
  // Get the base_instance instance
  auto& base_instance = GalbotRobot::get_instance(MachineType::G1);

  if (!base_instance.init()) {
    std::cout << "base_instance init fail" << std::endl;
    return -1;
  }

  // Waiting for init done
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

  printCommandHelp();

  std::string cmd, stream_id;
  while (base_instance.is_running()) {
    std::cout << "Enter interface name to test:";
    std::string input_line;
    std::getline(std::cin, input_line);

    // Extract the first word as the command (ignore subsequent arguments)
    std::istringstream iss(input_line);
    iss >> cmd;

    // If input is empty, continue the loop
    if (cmd.empty()) {
      continue;
    }
    if (cmd == "help") {
      printCommandHelp();
      continue;
    } else if (cmd == "start_microphone_stream_input") {
      stream_id = base_instance.start_microphone_stream_input([](const std::shared_ptr<AudioData> audio_data){
        handle_audio_data(audio_data);
      });

      std::cout << "Start microphone stream input, stream_id: "<< stream_id << std::endl;
    } else if (cmd == "stop_microphone_stream_input") {
      base_instance.stop_microphone_stream_input(stream_id);
      std::cout << "Stop microphone stream input, stream_id : " << stream_id << std::endl;
    } else if (cmd == "write_audio_stream_output") {
      std::string play_file;
      safe_read_file_name(play_file, "Please enter audio file name to play (16K 16bit Mono PCM audio file path): ");

      const auto chunk_size = 2560; // Data block size of 2560 bytes
      std::vector<char> audio_chunk(chunk_size, 0);
      std::cout << "Write audio stream data..." << std::endl;
      std::ifstream infile(play_file, std::ios::binary);
      if (!infile) {
        std::cout << "Failed to open for reading: " << play_file << std::endl;
        continue;
      } else {
        stream_id = "spk_stream_output_test";
        while (infile) {
          infile.read(audio_chunk.data(), static_cast<std::streamsize>(chunk_size));
          std::streamsize bytes_read = infile.gcount();
          if (bytes_read <= 0) break;
          std::string out_chunk(audio_chunk.data(), bytes_read);
          base_instance.write_audio_stream_output(out_chunk, stream_id);
          std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }
      }
      std::cout << "Write audio stream output, stream id : " << stream_id << std::endl;
    } else if (cmd == "stop_audio_stream_output") {
      base_instance.stop_audio_stream_output(stream_id);
      std::cout << "Stop audio stream output, stream id: " << stream_id << std::endl;
    } else if (cmd == "set_volume") {
      float volume = 60;
      if (!safe_read_number(volume, "Please enter volume setting parameter (0-100): ")) {
        continue;
      }

      if (!base_instance.set_volume(volume)) {
        std::cout << "Failed to set volume: " << volume << std::endl;
        continue;
      }
      std::cout << "Set current volume: " << volume << std::endl;
    } else if (cmd == "get_volume") {
      auto volume = base_instance.get_volume();
      std::cout << "Get current volume: " << volume << std::endl;
    } else if (cmd == "q") {
      break;
    } else {
      std::cout << "Invalid input" << std::endl;
    }
  }

  base_instance.request_shutdown();
  base_instance.wait_for_shutdown();
  base_instance.destroy();

  std::cout << "proccess end" << std::endl;
  return 0;
}

控制器管理

适用场景：查询控制器状态，并切换或管理机器人各部件使用的控制器。

examples/cpp/galbot_robot/src/controller_management_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_active_controller(const std::string& group_name, const std::string& controller_name) {
  std::cout << "Active controller for " << group_name << ": " << controller_name << std::endl;
}

// Helper function to print control status
void print_status(const std::string& operation, ControlStatus status) {
  std::cout << operation << ": ";
  if (status == ControlStatus::SUCCESS) {
    std::cout << "SUCCESS";
  } else {
    std::cout << "FAILED (Code: " << static_cast<int>(status) << ")";
  }
  std::cout << std::endl;
}

int main() {
  // Get robot instance
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize system
  std::cout << "Initializing robot..." << std::endl;
  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  // Wait for data readiness
  std::this_thread::sleep_for(std::chrono::seconds(1));

  // Define the controller we want to test
  // Using LEFT_ARM_PVT_CTRL as the test subject
  const std::string test_controller = G1ControllerName::LEFT_ARM_PVT_CTRL;
  const std::string controller_str = G1ControllerName::LEFT_ARM_PVT_CTRL;
  const std::string group_str = "left_arm";

  std::cout << "\n--- Starting Controller Management Example ---\n" << std::endl;

  // 1. Get active controller before switching
  std::string active_controller = robot.get_active_controller(group_str);
  print_active_controller(group_str, active_controller);

  // 2. Switch controller
  // Demonstrates switch_controller interface
  // This ensures the controller is acquired and started
  std::cout << "\n[Test 1] Switching to " << controller_str << "..." << std::endl;
  ControlStatus status = robot.switch_controller(test_controller);
  print_status("switch_controller", status);
  
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  active_controller = robot.get_active_controller(group_str);
  print_active_controller(group_str, active_controller);

  // 3. Stop controller
  // Demonstrates stop_controller interface
  std::cout << "\n[Test 2] Stopping controller for group " << group_str << "..." << std::endl;
  status = robot.stop_controller(group_str);
  print_status("stop_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  // 4. Release controller
  // Demonstrates release_controller interface
  std::cout << "\n[Test 3] Releasing controller for group " << group_str << "..." << std::endl;
  status = robot.release_controller(group_str);
  print_status("release_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  // 5. Acquire controller
  // Demonstrates acquire_controller interface
  std::cout << "\n[Test 4] Acquiring " << controller_str << "..." << std::endl;
  status = robot.acquire_controller(test_controller);
  print_status("acquire_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  // 6. Start controller
  // Demonstrates start_controller interface
  std::cout << "\n[Test 5] Starting controller for group " << group_str << "..." << std::endl;
  status = robot.start_controller(group_str);
  print_status("start_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  std::cout << "\n--- Controller Management Example Finished ---\n" << std::endl;

  // Shutdown
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

获取并设置灵巧手状态（get_dexhand_state && set_dexhand_command）

适用场景：读取已配置灵巧手的关节状态和运行状态并向已配置的灵巧手发送目标关节指令。

examples/cpp/galbot_robot/src/get_set_dexhand_state_example.cpp

#include <algorithm>
#include <chrono>
#include <cctype>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr int kCycleCount = 3;
constexpr int kStartupDelayMs = 2000;
constexpr int kCommandIntervalMs = 1000;

// These demo positions match the standalone set_dexhand_command example.
// Each dexterous hand model has its own joint count and position unit.
const std::vector<double> INSPIRE_RH56DFX_POSITION_OPEN = {
    1000.0,  // finger1
    1000.0,  // finger2
    1000.0,  // finger3
    1000.0,  // finger4
    1000.0,  // finger5
    1000.0,  // finger6
};

const std::vector<double> INSPIRE_RH56DFX_POSITION_CLOSE = {
    850.0,  // finger1
    350.0,  // finger2
    60.0,   // finger3
    60.0,   // finger4
    60.0,   // finger5
    60.0,   // finger6
};

const std::vector<double> INSPIRE_RH56F2_POSITION_OPEN = {
    1750.0,  // thumb_bend
    1550.0,  // thumb_rotate
    1740.0,  // index
    1740.0,  // middle
    1740.0,  // ring
    1740.0,  // little
};

const std::vector<double> INSPIRE_RH56F2_POSITION_CLOSE = {
    1600.0,  // thumb_bend
    1250.0,  // thumb_rotate
    950.0,   // index
    950.0,   // middle
    950.0,   // ring
    950.0,   // little
};

const std::vector<double> BRAINCO_POSITION_OPEN = {
    100.0,  // finger1
    100.0,  // finger2
    100.0,  // finger3
    100.0,  // finger4
    100.0,  // finger5
    100.0,  // finger6
};

const std::vector<double> BRAINCO_POSITION_CLOSE = {
    33.3,   // finger1
    100.0,  // finger2
    86.7,   // finger3
    86.7,   // finger4
    86.7,   // finger5
    86.7,   // finger6
};

const std::vector<double> LINKER_L20_POSITION_OPEN = {
    1.3543754995475996,   // thumb_roll
    1.562069680534925,    // thumb_yaw
    0.23561944901923448,  // index_yaw
    0.23561944901923448,  // middle_yaw
    0.23561944901923448,  // ring_yaw
    0.23561944901923448,  // little_yaw
    0.82030474843733492,  // thumb_root
    1.2217304763960306,   // index_root
    1.2217304763960306,   // middle_root
    1.2217304763960306,   // ring_root
    1.2217304763960306,   // little_root
    1.2042771838760873,   // thumb_tip
    1.7453292519943295,   // index_tip
    1.7453292519943295,   // middle_tip
    1.7453292519943295,   // ring_tip
    1.7453292519943295,   // little_tip
};

const std::vector<double> LINKER_L20_POSITION_CLOSE = {
    0.85,  // thumb_roll
    1.55,  // thumb_yaw
    0.00,  // index_yaw
    0.00,  // middle_yaw
    0.00,  // ring_yaw
    0.00,  // little_yaw
    0.40,  // thumb_root
    0.00,  // index_root
    0.00,  // middle_root
    0.00,  // ring_root
    0.00,  // little_root
    0.35,  // thumb_tip
    0.00,  // index_tip
    0.00,  // middle_tip
    0.00,  // ring_tip
    0.00,  // little_tip
};

const std::vector<double> SHARPA_POSITION_OPEN = {
    0.0,  // thumb CMC_FE
    0.0,  // thumb CMC_AA
    0.0,  // thumb MCP_FE
    0.0,  // thumb MCP_AA
    0.0,  // thumb IP
    0.0,  // index MCP_FE
    0.0,  // index MCP_AA
    0.0,  // index PIP
    0.0,  // index DIP
    0.0,  // middle MCP_FE
    0.0,  // middle MCP_AA
    0.0,  // middle PIP
    0.0,  // middle DIP
    0.0,  // ring MCP_FE
    0.0,  // ring MCP_AA
    0.0,  // ring PIP
    0.0,  // ring DIP
    0.0,  // pinky CMC
    0.0,  // pinky MCP_FE
    0.0,  // pinky MCP_AA
    0.0,  // pinky PIP
    0.0,  // pinky DIP
};

const std::vector<double> SHARPA_POSITION_CLOSE = {
    0.87,  // thumb CMC_FE
    0.0,   // thumb CMC_AA
    0.44,  // thumb MCP_FE
    0.0,   // thumb MCP_AA
    0.87,  // thumb IP
    0.78,  // index MCP_FE
    0.0,   // index MCP_AA
    0.87,  // index PIP
    0.70,  // index DIP
    0.78,  // middle MCP_FE
    0.0,   // middle MCP_AA
    0.87,  // middle PIP
    0.70,  // middle DIP
    0.78,  // ring MCP_FE
    0.0,   // ring MCP_AA
    0.87,  // ring PIP
    0.70,  // ring DIP
    0.13,  // pinky CMC
    0.78,  // pinky MCP_FE
    0.0,   // pinky MCP_AA
    0.87,  // pinky PIP
    0.70,  // pinky DIP
};

std::string to_lower(std::string value) {
  std::transform(value.begin(), value.end(), value.begin(), [](unsigned char ch) {
    return static_cast<char>(std::tolower(ch));
  });
  return value;
}

bool parse_dexhand_type(const std::string& type_name, DexHandType& dexhand_type) {
  const std::string key = to_lower(type_name);
  if (key == "inspire") {
    dexhand_type = DexHandType::INSPIRE;
    return true;
  }
  if (key == "inspire_rh56dfx") {
    dexhand_type = DexHandType::INSPIRE_RH56DFX;
    return true;
  }
  if (key == "inspire_rh56f2") {
    dexhand_type = DexHandType::INSPIRE_RH56F2;
    return true;
  }
  if (key == "brainco" || key == "revo") {
    dexhand_type = DexHandType::BRAINCO;
    return true;
  }
  if (key == "sharpa") {
    dexhand_type = DexHandType::SHARPA;
    return true;
  }
  if (key == "linker_l20") {
    dexhand_type = DexHandType::LINKER_L20;
    return true;
  }
  return false;
}

bool is_exit_input(const std::string& value) {
  const std::string key = to_lower(value);
  return key == "q" || key == "quit" || key == "exit";
}

void print_supported_types() {
  std::cout << "Supported dexterous hand types:" << std::endl;
  std::cout << "  inspire          Compatibility alias for inspire_rh56f2" << std::endl;
  std::cout << "  inspire_rh56dfx  Inspire RH56DFX dexterous hand" << std::endl;
  std::cout << "  inspire_rh56f2   Inspire RH56F2 dexterous hand" << std::endl;
  std::cout << "  brainco          BrainCo dexterous hand" << std::endl;
  std::cout << "  revo             Alias for brainco" << std::endl;
  std::cout << "  sharpa           Sharpa dexterous hand" << std::endl;
  std::cout << "  linker_l20       Linker Hand L20 dexterous hand" << std::endl;
}

bool read_dexhand_type(const std::string& side_name, DexHandType& dexhand_type, std::string& type_label) {
  while (true) {
    std::cout << "Enter " << side_name << " dexterous hand type (or q to quit): ";
    std::string raw_value;
    if (!std::getline(std::cin, raw_value)) {
      return false;
    }

    type_label = to_lower(raw_value);
    if (is_exit_input(type_label)) {
      return false;
    }
    if (parse_dexhand_type(type_label, dexhand_type)) {
      return true;
    }

    std::cout << "Unsupported dexterous hand type: " << raw_value << std::endl;
    std::cout << "Please choose from: inspire, inspire_rh56dfx, inspire_rh56f2, brainco, revo, sharpa, linker_l20"
              << std::endl;
  }
}

std::vector<JointCommand> make_dexhand_command(const std::vector<double>& positions) {
  std::vector<JointCommand> commands(positions.size());
  for (size_t i = 0; i < positions.size(); ++i) {
    commands[i].position = positions[i];
  }
  return commands;
}

const std::vector<double>& dexhand_motion_positions(DexHandType dexhand_type, const std::string& action) {
  const bool is_open = action == "open";
  if (dexhand_type == DexHandType::INSPIRE_RH56DFX) {
    return is_open ? INSPIRE_RH56DFX_POSITION_OPEN : INSPIRE_RH56DFX_POSITION_CLOSE;
  }
  if (dexhand_type == DexHandType::INSPIRE || dexhand_type == DexHandType::INSPIRE_RH56F2) {
    return is_open ? INSPIRE_RH56F2_POSITION_OPEN : INSPIRE_RH56F2_POSITION_CLOSE;
  }
  if (dexhand_type == DexHandType::BRAINCO) {
    return is_open ? BRAINCO_POSITION_OPEN : BRAINCO_POSITION_CLOSE;
  }
  if (dexhand_type == DexHandType::SHARPA) {
    return is_open ? SHARPA_POSITION_OPEN : SHARPA_POSITION_CLOSE;
  }
  return is_open ? LINKER_L20_POSITION_OPEN : LINKER_L20_POSITION_CLOSE;
}

void print_dexhand_state(const std::string& hand_name, const DexhandState& dexhand_state,
                         DexHandType dexhand_type) {
  const char* type_label = dexhand_type == DexHandType::SHARPA ? "sharpa" : "dexterous";
  std::cout << hand_name << " " << type_label << " hand state:" << std::endl;
  std::cout << "Timestamp (ns): " << dexhand_state.timestamp_ns << std::endl;

  const auto& joint_state_vec = dexhand_state.joint_state.joint_state_vec;
  std::cout << "  Joint states (" << joint_state_vec.size() << " joints):" << std::endl;
  for (size_t i = 0; i < joint_state_vec.size(); ++i) {
    const auto& js = joint_state_vec[i];
    if (dexhand_type == DexHandType::SHARPA) {
      // Sharpa state does not use the generic acceleration field in the same way.
      std::cout << "    joint" << (i + 1) << ": position=" << js.position << ", velocity=" << js.velocity
                << ", effort=" << js.effort << ", current=" << js.current << std::endl;
    } else {
      // Some SDK backends fill joint_name; otherwise keep a stable fallback label.
      const std::string joint_label = !js.joint_name.empty()
                                          ? js.joint_name
                                          : hand_name + "_dexhand_joint" + std::to_string(i + 1);
      std::cout << "    " << joint_label << ": position=" << js.position
                << ", velocity=" << js.velocity << ", acceleration=" << js.acceleration
                << ", effort=" << js.effort << ", current=" << js.current << std::endl;
    }
  }

  if (dexhand_type != DexHandType::SHARPA) {
    return;
  }

  const std::unordered_map<std::string, EffortInfo>& force_sensor_map = dexhand_state.force_sensor_map;
  if (force_sensor_map.empty()) {
    std::cout << "  (no force sensor data)" << std::endl;
    return;
  }

  std::cout << "  Force sensors (" << force_sensor_map.size() << " sensors):" << std::endl;
  for (const auto& sensor_pair : force_sensor_map) {
    const auto& effort = sensor_pair.second;
    std::cout << "    " << sensor_pair.first << " @ " << effort.timestamp_ns
              << ": Fx=" << effort.force.x << ", Fy=" << effort.force.y << ", Fz=" << effort.force.z
              << ", Mx=" << effort.torque.x << ", My=" << effort.torque.y << ", Mz=" << effort.torque.z
              << std::endl;
  }
}

}  // namespace

int main() {
  std::cout << "Starting get_set_dexhand_state example" << std::endl;
  print_supported_types();

  // Ask the user to select the actual dexterous hand model installed on each side.
  DexHandType left_type;
  std::string left_label;
  if (!read_dexhand_type("left", left_type, left_label)) {
    std::cout << "Example canceled before robot initialization" << std::endl;
    return 0;
  }

  DexHandType right_type;
  std::string right_label;
  if (!read_dexhand_type("right", right_type, right_label)) {
    std::cout << "Example canceled before robot initialization" << std::endl;
    return 0;
  }

  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize the robot SDK before sending commands or reading states.
  if (!robot.init()) {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  std::cout << "Initialization succeeded" << std::endl;
  std::this_thread::sleep_for(std::chrono::milliseconds(kStartupDelayMs));

  bool completed = true;

  // Run one initial open command, then run three close/open demo cycles.
  for (int action_index = 0; action_index < 1 + kCycleCount * 2; ++action_index) {
    const std::string action = (action_index == 0 || action_index % 2 == 0) ? "open" : "close";
    if (action_index > 0 && action == "close") {
      const int cycle_index = (action_index + 1) / 2;
      std::cout << "Running dexterous hand cycle " << cycle_index << "/" << kCycleCount << std::endl;
    }

    // Send the target position command to the left dexterous hand.
    const auto left_positions = dexhand_motion_positions(left_type, action);
    const auto left_command = make_dexhand_command(left_positions);
    const ControlStatus left_status = robot.set_dexhand_command(
        "left_dexhand",
        left_command,
        left_type,
        false);
    if (left_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to " << action << " left " << left_label
                << " dexterous hand (" << left_command.size() << " joints), status="
                << static_cast<int>(left_status) << std::endl;
      completed = false;
    } else {
      std::cout << "Left " << left_label << " dexterous hand " << action
                << " command sent (" << left_command.size() << " joints)" << std::endl;
    }

    // Send the same action to the right dexterous hand with its own model type.
    const auto right_positions = dexhand_motion_positions(right_type, action);
    const auto right_command = make_dexhand_command(right_positions);
    const ControlStatus right_status = robot.set_dexhand_command(
        "right_dexhand",
        right_command,
        right_type,
        false);
    if (right_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to " << action << " right " << right_label
                << " dexterous hand (" << right_command.size() << " joints), status="
                << static_cast<int>(right_status) << std::endl;
      completed = false;
    } else {
      std::cout << "Right " << right_label << " dexterous hand " << action
                << " command sent (" << right_command.size() << " joints)" << std::endl;
    }

    // Give the hardware or simulator time to execute the command before reading state.
    std::this_thread::sleep_for(std::chrono::milliseconds(kCommandIntervalMs));

    // Read back the left hand state after the command has been applied.
    DexhandState left_state;
    const ControlStatus left_state_status = robot.get_dexhand_state("left_dexhand", left_state, left_type);
    if (left_state_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to get left dexterous hand state!" << std::endl;
      completed = false;
    } else {
      print_dexhand_state("Left", left_state, left_type);
    }

    // Read back the right hand state after the command has been applied.
    DexhandState right_state;
    const ControlStatus right_state_status = robot.get_dexhand_state("right_dexhand", right_state, right_type);
    if (right_state_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to get right dexterous hand state!" << std::endl;
      completed = false;
    } else {
      print_dexhand_state("Right", right_state, right_type);
    }
  }

  if (completed) {
    std::cout << "get_set_dexhand_state example completed" << std::endl;
  } else {
    std::cout << "get_set_dexhand_state example completed with one or more errors" << std::endl;
  }

  // Release SDK resources after the initialized example finishes or fails.
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released successfully" << std::endl;

  return completed ? 0 : 1;
}

获取力传感器数据（get_force_sensor_data）

适用场景：获取力和力矩测量值，用于接触检测及力感知操作。

examples/cpp/galbot_robot/src/get_force_sensor_data_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

std::string force_sensor_type_to_string(GalbotOneFoxtrotSensor sensor_type) {
    switch (sensor_type) {
        case GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE:
            return "LEFT_WRIST_FORCE";
        case GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE:
            return "RIGHT_WRIST_FORCE";
        default:
            return "UNKNOWN_FORCE_SENSOR";
    }
}

void print_force_data(GalbotOneFoxtrotSensor sensor_type, const std::shared_ptr<ForceData>& force_data) {
    std::cout << "--- " << force_sensor_type_to_string(sensor_type) << " ---" << std::endl;
    if (!force_data) {
        std::cerr << "  Force data is empty" << std::endl;
        return;
    }

    std::cout << "  Timestamp (ns): " << force_data->timestamp_ns << std::endl;
    std::cout << "  Force (N):  "
              << "fx=" << force_data->force.x << ", "
              << "fy=" << force_data->force.y << ", "
              << "fz=" << force_data->force.z << std::endl;
    std::cout << "  Torque (Nm): "
              << "tx=" << force_data->torque.x << ", "
              << "ty=" << force_data->torque.y << ", "
              << "tz=" << force_data->torque.z << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get left wrist force sensor data
    std::cout << "\n===== Get left wrist force sensor data =====" << std::endl;
    auto left_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE);
    print_force_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE, left_force_data);

    // Get calibrated left-arm wrench expressed in base_link
    std::cout << "\n===== Get calibrated left wrist force data in base_link =====" << std::endl;
    auto calibrated_left_force_data =
        robot.get_force_sensor_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE, true, "base_link");
    print_force_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE, calibrated_left_force_data);

    // Get right wrist force sensor data
    std::cout << "\n===== Get right wrist force sensor data =====" << std::endl;
    auto right_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE);
    print_force_data(GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE, right_force_data);

    // Keep calibrated right-arm wrench in its native end-effector mount frame
    std::cout << "\n===== Get calibrated right wrist force data in mount frame =====" << std::endl;
    auto calibrated_right_force_data = robot.get_force_sensor_data(
        GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE, true, "right_arm_end_effector_mount_link");
    print_force_data(GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE, calibrated_right_force_data);

    // Iterate and get all force sensor data
    std::cout << "\n===== Get all force sensor data =====" << std::endl;
    for (int i = 0; i < static_cast<int>(GalbotOneFoxtrotSensor::FORCE_NUM); ++i) {
        auto sensor_type = static_cast<GalbotOneFoxtrotSensor>(i);
        auto force_data = robot.get_force_sensor_data(sensor_type);
        print_force_data(sensor_type, force_data);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取里程计数据（get_odom）

适用场景：读取里程计报告的机器人底盘位姿和运动状态。

examples/cpp/galbot_robot/src/get_odom_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_odom_data(const std::shared_ptr<OdomData>& odom_data) {
    if (!odom_data) {
        std::cerr << "Odom data is empty" << std::endl;
        return;
    }

    std::cout << "Timestamp (ns): " << odom_data->timestamp_ns << std::endl;

    std::cout << "Position (m): "
              << "x=" << odom_data->position[0] << ", "
              << "y=" << odom_data->position[1] << ", "
              << "z=" << odom_data->position[2] << std::endl;

    std::cout << "Orientation (quaternion): "
              << "qx=" << odom_data->orientation[0] << ", "
              << "qy=" << odom_data->orientation[1] << ", "
              << "qz=" << odom_data->orientation[2] << ", "
              << "qw=" << odom_data->orientation[3] << std::endl;

    std::cout << "Linear velocity (m/s): "
              << "vx=" << odom_data->linear_velocity[0] << ", "
              << "vy=" << odom_data->linear_velocity[1] << ", "
              << "vz=" << odom_data->linear_velocity[2] << std::endl;

    std::cout << "Angular velocity (rad/s): "
              << "wx=" << odom_data->angular_velocity[0] << ", "
              << "wy=" << odom_data->angular_velocity[1] << ", "
              << "wz=" << odom_data->angular_velocity[2] << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get odometry data
    std::shared_ptr<OdomData> odom_data = robot.get_odom();
    if (odom_data) {
        std::cout << "Odometry data retrieved successfully!" << std::endl;
        print_odom_data(odom_data);
    } else {
        std::cerr << "Failed to get odometry data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取超声波数据（get_ultrasonic_data）

适用场景：读取超声波测距数据，用于近距离障碍物感知。

examples/cpp/galbot_robot/src/get_ultrasonic_data_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include <unordered_set>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

std::string ultrasonic_type_to_string(UltrasonicType ultrasonic_type) {
    switch (ultrasonic_type) {
        case UltrasonicType::FRONT_LEFT:
            return "FRONT_LEFT";
        case UltrasonicType::FRONT_RIGHT:
            return "FRONT_RIGHT";
        case UltrasonicType::RIGHT_LEFT:
            return "RIGHT_LEFT";
        case UltrasonicType::RIGHT_RIGHT:
            return "RIGHT_RIGHT";
        case UltrasonicType::BACK_LEFT:
            return "BACK_LEFT";
        case UltrasonicType::BACK_RIGHT:
            return "BACK_RIGHT";
        case UltrasonicType::LEFT_LEFT:
            return "LEFT_LEFT";
        case UltrasonicType::LEFT_RIGHT:
            return "LEFT_RIGHT";
        default:
            return "UNKNOWN_ULTRASONIC";
    }
}

void print_ultrasonic_data(UltrasonicType ultrasonic_type, const std::shared_ptr<UltrasonicData>& ultrasonic_data) {
    std::cout << "--- " << ultrasonic_type_to_string(ultrasonic_type) << " ---" << std::endl;
    if (!ultrasonic_data) {
        std::cerr << "  Ultrasonic data is empty" << std::endl;
        return;
    }

    std::cout << "  Timestamp (ns): " << ultrasonic_data->timestamp_ns << std::endl;
    std::cout << "  Distance (m): " << ultrasonic_data->distance << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize sensors; only sensors passed during initialization can retrieve data to save resources
    std::unordered_set<SensorType> sensor_types = {
        SensorType::BASE_ULTRASONIC  // Chassis ultrasonic sensor
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get single ultrasonic sensor data
    std::cout << "\n===== Get single ultrasonic sensor data =====" << std::endl;
    auto ultrasonic_data = robot.get_ultrasonic_data(UltrasonicType::FRONT_LEFT);
    print_ultrasonic_data(UltrasonicType::FRONT_LEFT, ultrasonic_data);

    // Iterate to get all 8 ultrasonic sensor data
    std::cout << "\n===== Get all ultrasonic sensor data =====" << std::endl;
    for (int i = 0; i < static_cast<int>(UltrasonicType::ULTRASONIC_NUM); ++i) {
        auto ultrasonic_type = static_cast<UltrasonicType>(i);
        auto data = robot.get_ultrasonic_data(ultrasonic_type);
        print_ultrasonic_data(ultrasonic_type, data);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

安装验证

适用场景：验证 SDK 安装、机器人连接、初始化以及基础接口是否可用。

examples/cpp/galbot_robot/src/installation_welcome_verify.cpp

#include <chrono>
#include <condition_variable>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <unordered_set>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

constexpr double kSpeedRadS = 0.12;
constexpr double kTimeoutS = 20.0;
constexpr auto kVideoStreamTimeout = std::chrono::seconds(10);
const std::string kWelcomePcmRel{};

constexpr double kHeadArmsDemoDtS = 0.06;
constexpr int kHeadArmsDemoSeg1 = 28;
constexpr int kHeadArmsDemoSeg2 = 32;
constexpr int kHeadArmsDemoSeg3 = 28;
constexpr double kHeadSwayYawPosRad = 0.22;
constexpr double kHeadSwayYawNegRad = -0.22;

const std::vector<double> kPresetLeg = {0.5, 1.5, 1.0, 0.0, 0.0};
const std::vector<double> kPresetHead = {0.0, 0.0};
const std::vector<double> kPresetLeftArm = {2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0};
const std::vector<double> kPresetRightArm = {-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0};

const std::vector<std::string> kHeadArmsDemoGroups = {"head", "left_arm", "right_arm"};

TrajectoryPoint make_point(const std::vector<double>& joint_positions_rad, double time_from_start_sec) {
  TrajectoryPoint point;
  point.time_from_start_second = time_from_start_sec;
  for (double q : joint_positions_rad) {
    JointCommand jc;
    jc.position = q;
    point.joint_command_vec.push_back(jc);
  }
  return point;
}

Trajectory build_trajectory(const std::vector<std::vector<double>>& joint_waypoints_rad,
                            const std::vector<std::string>& joint_groups, double time_step_sec) {
  Trajectory trajectory;
  trajectory.joint_groups = joint_groups;
  trajectory.joint_names = {};
  double time_from_start_sec = 0.0;
  for (const std::vector<double>& waypoint : joint_waypoints_rad) {
    time_from_start_sec += time_step_sec;
    trajectory.points.push_back(make_point(waypoint, time_from_start_sec));
  }
  return trajectory;
}

std::vector<std::vector<double>> linspace_rows(const std::vector<double>& start_positions_rad,
                                               const std::vector<double>& end_positions_rad, int num_samples) {
  std::vector<std::vector<double>> out;
  if (num_samples < 2) {
    out.push_back(start_positions_rad);
    return out;
  }
  out.reserve(static_cast<size_t>(num_samples));
  const int n = num_samples - 1;
  for (int sample_idx = 0; sample_idx < num_samples; ++sample_idx) {
    const double blend = static_cast<double>(sample_idx) / static_cast<double>(n);
    std::vector<double> row(start_positions_rad.size());
    for (size_t j = 0; j < start_positions_rad.size(); ++j) {
      row[j] = start_positions_rad[j] + blend * (end_positions_rad[j] - start_positions_rad[j]);
    }
    out.push_back(std::move(row));
  }
  return out;
}

void append_rows(std::vector<std::vector<double>>& dest, const std::vector<std::vector<double>>& src) {
  dest.insert(dest.end(), src.begin(), src.end());
}

std::vector<double> upper_body_preset_pose_rad() {
  std::vector<double> pose;
  pose.reserve(kPresetHead.size() + kPresetLeftArm.size() + kPresetRightArm.size());
  pose.insert(pose.end(), kPresetHead.begin(), kPresetHead.end());
  pose.insert(pose.end(), kPresetLeftArm.begin(), kPresetLeftArm.end());
  pose.insert(pose.end(), kPresetRightArm.begin(), kPresetRightArm.end());
  return pose;
}

// Slow head yaw left-right (joint2 fixed); arms shift slightly with the sway. Legs not in trajectory.
std::vector<std::vector<double>> build_slow_head_arms_demo_waypoints_rad() {
  const std::vector<double> home = upper_body_preset_pose_rad();
  const std::vector<double> pose_yaw_pos = {
    kHeadSwayYawPosRad, 0.0,
    2.0 + 0.07, -1.5 + 0.04, -0.6, -1.7, 0.0, -0.8 + 0.04, 0.0,
    -2.0 - 0.07, 1.5 - 0.04, 0.6, 1.7, 0.0, 0.8 - 0.04, 0.0
  };
  const std::vector<double> pose_yaw_neg = {
    kHeadSwayYawNegRad, 0.0,
    2.0 - 0.05, -1.5 - 0.03, -0.6, -1.7, 0.0, -0.8 - 0.03, 0.0,
    -2.0 + 0.05, 1.5 + 0.03, 0.6, 1.7, 0.0, 0.8 + 0.03, 0.0
  };

  std::vector<std::vector<double>> rows;
  append_rows(rows, linspace_rows(home, pose_yaw_pos, kHeadArmsDemoSeg1));
  append_rows(rows, linspace_rows(pose_yaw_pos, pose_yaw_neg, kHeadArmsDemoSeg2));
  append_rows(rows, linspace_rows(pose_yaw_neg, home, kHeadArmsDemoSeg3));
  return rows;
}

std::string getenv_string(const std::string& key) {
  const char* v = std::getenv(key.c_str());
  return (v != nullptr && v[0] != '\0') ? std::string(v) : std::string();
}

std::string resolve_welcome_pcm_path() {
  const std::string env_pcm = getenv_string("GALBOT_WELCOME_PCM");
  if (!env_pcm.empty()) {
    return env_pcm;
  }
  return kWelcomePcmRel;
}

bool play_pcm(GalbotRobot& robot, const std::string& pcm_file_path) {
  constexpr std::size_t kPcmReadChunkBytes = 2560;
  const std::string audio_stream_id = "install_welcome_pcm";
  std::ifstream pcm_input(pcm_file_path, std::ios::binary);
  if (!pcm_input) {
    std::cerr << "[audio] Failed to open/read PCM: " << pcm_file_path << std::endl;
    return false;
  }
  std::vector<char> buf(kPcmReadChunkBytes);
  while (pcm_input) {
    pcm_input.read(buf.data(), static_cast<std::streamsize>(kPcmReadChunkBytes));
    const std::streamsize n = pcm_input.gcount();
    if (n <= 0) {
      break;
    }
    const std::string chunk(buf.data(), static_cast<std::size_t>(n));
    if (!robot.write_audio_stream_output(chunk, audio_stream_id)) {
      std::cerr << "[audio] write_audio_stream_output failed" << std::endl;
      return false;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
  }
  return true;
}

void print_summary(const std::string& step_description, bool step_passed) {
  std::cout << (step_passed ? "[PASS] " : "[FAIL] ") << step_description << std::endl;
}

// Verify that the head camera publishes a non-empty H.264 frame without writing
// installation-test data to disk. Shared state keeps callback data valid even if
// the SDK dispatch thread finishes the callback while unsubscription is running.
bool verify_head_h264_stream(GalbotRobot& robot) {
  struct VideoCheckState {
    std::mutex mutex;
    std::condition_variable frame_received;
    bool has_valid_frame{false};
    std::string format;
    std::size_t frame_size_bytes{0};
  };

  const auto state = std::make_shared<VideoCheckState>();
  const SensorStatus subscribe_status = robot.subscribe_video_data(
      SensorType::HEAD_LEFT_CAMERA, [state](const std::shared_ptr<EncodedVideoData>& video_data) {
        // Accept both the documented lowercase spelling and the uppercase middleware payload.
        if (video_data == nullptr || video_data->data.empty() ||
            (video_data->format != "h264" && video_data->format != "H264")) {
          return;
        }

        {
          std::lock_guard<std::mutex> lock(state->mutex);
          state->has_valid_frame = true;
          state->format = video_data->format;
          state->frame_size_bytes = video_data->data.size();
        }
        state->frame_received.notify_one();
      });
  if (subscribe_status != SensorStatus::SUCCESS) {
    std::cerr << "[FAIL] subscribe_video_data failed, status=" << static_cast<int>(subscribe_status) << std::endl;
    return false;
  }

  bool received_valid_frame = false;
  std::string received_format;
  std::size_t received_frame_size_bytes = 0;
  {
    std::unique_lock<std::mutex> lock(state->mutex);
    received_valid_frame =
        state->frame_received.wait_for(lock, kVideoStreamTimeout, [&state]() { return state->has_valid_frame; });
    if (received_valid_frame) {
      received_format = state->format;
      received_frame_size_bytes = state->frame_size_bytes;
    }
  }

  // Always unsubscribe after a successful subscription, including timeout paths.
  const SensorStatus unsubscribe_status = robot.unsubscribe_video_data(SensorType::HEAD_LEFT_CAMERA);
  if (unsubscribe_status != SensorStatus::SUCCESS) {
    std::cerr << "[FAIL] unsubscribe_video_data failed, status=" << static_cast<int>(unsubscribe_status) << std::endl;
    return false;
  }
  if (!received_valid_frame) {
    std::cerr << "[FAIL] Timed out waiting for a non-empty H.264 frame" << std::endl;
    return false;
  }

  std::cout << "Head camera H.264 frame: " << received_frame_size_bytes << " bytes format=" << received_format
            << std::endl;
  return true;
}

int main() {
  bool body_preset_step_passed = false;
  bool welcome_pcm_playback_ok = false;
  bool head_video_stream_ok = false;

  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  const std::unordered_set<SensorType> required_sensors = {SensorType::HEAD_LEFT_CAMERA};
  if (!robot.init(required_sensors)) {
    std::cerr << "GalbotRobot::init failed" << std::endl;
    return 1;
  }
  std::cout << "Robot initialized (HEAD_LEFT_CAMERA enabled)" << std::endl;
  std::this_thread::sleep_for(std::chrono::duration<double>(5.0));

  if (!robot.set_volume(100.0f)) {
    std::cerr << "[WARN] set_volume(100) failed; welcome PCM may play quietly" << std::endl;
  }

  bool leg_step_ok = false;
  bool upper_step_ok = false;
  bool demo_step_ok = false;

  const ControlStatus leg_set_status =
      robot.set_joint_positions(kPresetLeg, {"leg"}, {}, true, kSpeedRadS, kTimeoutS);
  leg_step_ok = (leg_set_status == ControlStatus::SUCCESS);
  if (!leg_step_ok) {
    std::cerr << "[FAIL] Leg set_joint_positions not SUCCESS (blocking)" << std::endl;
  }
  print_summary("Leg set_joint_positions (blocking)", leg_step_ok);

  if (leg_step_ok) {
    std::vector<double> upper_body_joint_targets_rad;
    upper_body_joint_targets_rad.insert(upper_body_joint_targets_rad.end(), kPresetHead.begin(), kPresetHead.end());
    upper_body_joint_targets_rad.insert(upper_body_joint_targets_rad.end(), kPresetLeftArm.begin(),
                                        kPresetLeftArm.end());
    upper_body_joint_targets_rad.insert(upper_body_joint_targets_rad.end(), kPresetRightArm.begin(),
                                        kPresetRightArm.end());

    const ControlStatus upper_body_set_status =
        robot.set_joint_positions(upper_body_joint_targets_rad, kHeadArmsDemoGroups, {}, true, kSpeedRadS, kTimeoutS);
    upper_step_ok = (upper_body_set_status == ControlStatus::SUCCESS);
    if (!upper_step_ok) {
      std::cerr << "[FAIL] head/left_arm/right_arm set_joint_positions not SUCCESS (blocking)" << std::endl;
    }
    print_summary("Upper set_joint_positions head+arms (blocking)", upper_step_ok);

    if (upper_step_ok) {
      const std::vector<std::vector<double>> demo_waypoints_rad = build_slow_head_arms_demo_waypoints_rad();
      const Trajectory demo_trajectory = build_trajectory(demo_waypoints_rad, kHeadArmsDemoGroups, kHeadArmsDemoDtS);
      const ControlStatus demo_exec_status = robot.execute_joint_trajectory(demo_trajectory, true);
      demo_step_ok = (demo_exec_status == ControlStatus::SUCCESS);
      if (!demo_step_ok) {
        std::cerr << "[FAIL] Head/arms execute_joint_trajectory not SUCCESS (blocking)" << std::endl;
      }
      print_summary("Head/arms demo trajectory (blocking)", demo_step_ok);
    } else {
      std::cerr << "[INFO] Skipping head/arms demo: upper set_joint_positions did not return SUCCESS." << std::endl;
      print_summary("Head/arms demo trajectory (blocking)", false);
    }
  } else {
    std::cerr << "[INFO] Skipping upper-body preset and head/arms demo: leg set_joint_positions not SUCCESS."
              << std::endl;
    print_summary("Upper set_joint_positions head+arms (blocking)", false);
    print_summary("Head/arms demo trajectory (blocking)", false);
  }

  body_preset_step_passed = leg_step_ok && upper_step_ok && demo_step_ok;

  const std::string resolved_welcome_pcm_path = resolve_welcome_pcm_path();
  std::cout << "PCM path: " << resolved_welcome_pcm_path << std::endl;
  if (resolved_welcome_pcm_path.empty()) {
    std::cerr << "[FAIL] Welcome PCM path is empty: set GALBOT_WELCOME_PCM (no built-in default path)."
              << std::endl;
    welcome_pcm_playback_ok = false;
  } else {
    std::ifstream pcm_probe(resolved_welcome_pcm_path, std::ios::binary);
    if (!pcm_probe) {
      std::cerr << "[FAIL] PCM file not found or not readable: " << resolved_welcome_pcm_path << std::endl;
      welcome_pcm_playback_ok = false;
    } else {
      welcome_pcm_playback_ok = play_pcm(robot, resolved_welcome_pcm_path);
    }
  }
  print_summary("Welcome PCM playback", welcome_pcm_playback_ok);

  head_video_stream_ok = verify_head_h264_stream(robot);
  print_summary("Head camera H.264 video stream", head_video_stream_ok);

  std::cout << "\n======== Summary ========" << std::endl;
  print_summary("Motion verify (blocking)", body_preset_step_passed);
  print_summary("Audio", welcome_pcm_playback_ok);
  print_summary("Head camera H.264 video stream", head_video_stream_ok);

  const bool installation_all_checks_passed =
      body_preset_step_passed && welcome_pcm_playback_ok && head_video_stream_ok;
  std::cout << (installation_all_checks_passed ? "\nOverall: PASS\n" : "\nOverall: FAIL\n");

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return installation_all_checks_passed ? 0 : 1;
}

设置机器人配置（set_config）

适用场景：通过统一配置接口设置支持的机器人服务或运行参数。

examples/cpp/galbot_robot/src/set_config_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

// Full list of settable fields (navigation service, camera services, motion
// planning service and control service), their meaning, type and valid range:
// docs/g1/en/set_config_reference.md

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Navigation service: adjust replanning thresholds.
    std::vector<ConfigItem> navigation_fields = {
        ConfigItem{"replan_threshold", 30.0},
        ConfigItem{"no_replan_threshold", 1.0},
    };
    ControlStatus navigation_status = robot.set_config(ConfigService::NAVIGATION, navigation_fields);
    if (navigation_status == ControlStatus::SUCCESS) {
        std::cout << "[Navigation] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[Navigation] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Front head camera: set resolution.
    // color_width/color_height must be set together in the same call, and the
    // combination must be one of the documented valid resolutions.
    std::vector<ConfigItem> camera_fields = {
        ConfigItem{"color_width", static_cast<int64_t>(1280)},
        ConfigItem{"color_height", static_cast<int64_t>(992)},
    };
    ControlStatus camera_status = robot.set_config(ConfigService::FRONT_HEAD_CAMERA, camera_fields);
    if (camera_status == ControlStatus::SUCCESS) {
        std::cout << "[Camera] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[Camera] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Motion planning service: set a non-per-chain field (plan_timeout).
    std::vector<ConfigItem> motion_plan_fields = {
        ConfigItem{"plan_timeout", 5.0},
    };
    ControlStatus motion_plan_status = robot.set_config(ConfigService::MOTION_PLAN, motion_plan_fields);
    if (motion_plan_status == ControlStatus::SUCCESS) {
        std::cout << "[MotionPlan] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[MotionPlan] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Control service: report_error_skip is a general parameter shared by
    // both G-series and S1 (robot_config.toml).
    std::vector<ConfigItem> control_fields = {
        ConfigItem{"report_error_skip", static_cast<int64_t>(1)},
    };
    ControlStatus control_status = robot.set_config(ConfigService::CONTROL, control_fields);
    if (control_status == ControlStatus::SUCCESS) {
        std::cout << "[Control] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[Control] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

读取机器人配置（get_config）

适用场景：读取当前场景的持久化配置；也可读取机器人内置默认配置，用于对比当前配置与默认值。

examples/cpp/galbot_robot/src/get_config_example.cpp

#include <iostream>
#include <string>
#include <type_traits>
#include <variant>
#include <vector>
#include "galbot_robot.hpp"
using namespace galbot::sdk;
static void print_fields(const char* title, ControlStatus status, const std::vector<ConfigItem>& fields) {
  std::cout << "[" << title << "] status: " << static_cast<int>(status) << std::endl;
  for (const auto& field : fields) {
    std::cout << "  " << field.key << " = ";
    std::visit([](const auto& value) {
      using T = std::decay_t<decltype(value)>;
      if constexpr (std::is_arithmetic_v<T> || std::is_same_v<T, std::string>) std::cout << value;
      else std::cout << "[array, " << value.size() << " elements]";
    }, field.value);
    std::cout << std::endl;
  }
}
int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  if (!robot.init()) return -1;
  const std::vector<std::string> keys = {"report_error_skip", "report_sensor_skip"};
  std::vector<ConfigItem> fields;
  auto status = robot.get_config(ConfigService::CONTROL, keys, &fields);
  print_fields("Current selected values", status, fields);
  fields.clear();
  status = robot.get_config(ConfigService::CONTROL, keys, &fields, true);
  print_fields("Built-in default values (use_default=true)", status, fields);
  robot.request_shutdown(); robot.wait_for_shutdown(); robot.destroy();
  return 0;
}

设置腿部高度（set_leg_height）

适用场景：通过腿部机构调整机器人机身高度。

examples/cpp/galbot_robot/src/set_leg_height_example.cpp

#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_status(const std::string& operation, ControlStatus status) {
  if (status == ControlStatus::SUCCESS) {
    std::cout << operation << " succeeded." << std::endl;
  } else {
    std::cerr << operation << " failed, status: " << static_cast<int>(status) << std::endl;
  }
}

bool get_leg_height(GalbotRobot& robot, const std::string& label, double& height_m) {
  const auto [pose, timestamp_ns] = robot.get_transform("base_link", "head_base_link", 0, 500);
  std::cout << std::fixed << std::setprecision(3) << label << ": pose=[";
  for (size_t i = 0; i < 7; ++i) {
    std::cout << pose[i] << (i + 1 < 7 ? ", " : "");
  }
  std::cout << "], tf_timestamp_ns=" << timestamp_ns << std::endl;

  height_m = pose[2];
  return height_m >= 0.0;
}

bool confirm_leg_height_motion(double current_height_m, double target_height_m) {
  const char* direction = target_height_m < current_height_m ? "lower" : "raise";
  const double motion_distance_m = std::abs(target_height_m - current_height_m);
  std::cout << "The leg mechanism is about to " << direction << " the body by "
            << std::fixed << std::setprecision(3) << motion_distance_m
            << " m, from " << current_height_m << " m to " << target_height_m
            << " m. Confirm the surrounding environment is clear and safe to avoid collisions. "
            << "Enter y to continue; any other input cancels: ";
  std::string response;
  std::getline(std::cin, response);
  return response == "y" || response == "Y";
}

int main() {
  // Get and initialize the G1 robot instance.
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  std::cout << "Initializing robot..." << std::endl;
  if (!robot.init()) {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }
  std::cout << "System initialized successfully!" << std::endl;

  // Wait for WBC communication and state data to become ready.
  std::this_thread::sleep_for(std::chrono::seconds(2));

  constexpr double kHeightOffsetM = 0.20;
  constexpr double kDurationS = 4.0;

  double initial_height_m = 0.0;
  if (!get_leg_height(robot, "Initial leg height", initial_height_m)) {
    std::cerr << "Failed to get a valid initial leg height." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const double lowered_height_m = initial_height_m - kHeightOffsetM;
  if (!confirm_leg_height_motion(initial_height_m, lowered_height_m)) {
    std::cout << "Leg height motion cancelled. Exiting program." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
  }

  std::cout << "Lowering leg height by " << kHeightOffsetM
            << " m to " << lowered_height_m << " m." << std::endl;
  auto status = robot.set_leg_height(lowered_height_m, kDurationS, true);
  print_status("set_leg_height(lowered)", status);

  double actual_lowered_height_m = 0.0;
  if (!get_leg_height(robot, "Height after lowering", actual_lowered_height_m)) {
    std::cerr << "Failed to get the current leg height; cancelling the restore motion." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }
  std::cout << "Lowered target error: " << std::abs(actual_lowered_height_m - lowered_height_m)
            << " m." << std::endl;

  if (!confirm_leg_height_motion(actual_lowered_height_m, initial_height_m)) {
    std::cout << "Leg height restore motion cancelled. Exiting program." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
  }

  std::cout << "Restoring initial leg height: " << initial_height_m << " m." << std::endl;
  status = robot.set_leg_height(initial_height_m, kDurationS, true);
  print_status("set_leg_height(initial)", status);

  double final_height_m = 0.0;
  get_leg_height(robot, "Final leg height", final_height_m);
  std::cout << "Restored height error: " << std::abs(final_height_m - initial_height_m)
            << " m." << std::endl;

  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  std::cout << "switch_controller back to leg_pvt_ctrl" << std::endl;
  robot.switch_controller(G1ControllerName::LEG_PVT_CTRL);

  std::cout << "Waiting 1 second before shutdown..." << std::endl;
  std::this_thread::sleep_for(std::chrono::seconds(1));

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released successfully." << std::endl;
  return 0;
}

全身控制末端执行器命令（set_end_effector_command / clear_end_effector_command）

适用场景：需要根据高频模型推理结果实时控制末端笛卡尔位姿的场景。使用 clear_end_effector_command 可停止并清除当前末端实时控制指令。

examples/cpp/galbot_robot/src/set_end_effector_commands_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Wait for data to become available
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get current WBC end-effector poses
    // Keys: "ree_pose", "lee_pose", "head_pose" — each is [x, y, z, qx, qy, qz, qw]
    auto ee_info = robot.get_wbc_end_effector_poses();// Before using this function, ensure that `using_wbc` in the configuration file is set to `true`.
    std::cout << "\nCurrent WBC end-effector poses:" << std::endl;
    for (const auto& [frame, pose] : ee_info) {
        std::cout << "  Frame: " << frame << ", Pose: [";
        for (size_t i = 0; i < pose.size(); ++i) {
            std::cout << pose[i];
            if (i + 1 < pose.size()) std::cout << ", ";
        }
        std::cout << "]" << std::endl;
    }

    auto it = ee_info.find("ree_pose");
    if (it != ee_info.end() && it->second.size() >= 7) {
        std::vector<double> target_pose(it->second.begin(), it->second.begin() + 7);
        target_pose[0] -= 0.1;  // Move 10 cm in -X direction

        // Example: absolute pose — omit reference_frames to default every pose to "world"
        ControlStatus status = robot.set_end_effector_command(
            {target_pose},
            {"right_arm_end_effector_mount_link"}
        );

        if (status == ControlStatus::SUCCESS) {
            std::cout << "\nPublished end-effector command (default reference_frames -> world)" << std::endl;
        } else {
            std::cerr << "Failed to publish end-effector command!" << std::endl;
        }
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    // Clear end-effector commands
    ControlStatus clear_status = robot.clear_end_effector_command();
    if (clear_status == ControlStatus::SUCCESS) {
        std::cout << "End-effector commands cleared successfully!" << std::endl;
    } else {
        std::cerr << "Failed to clear end-effector commands!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << "Resource cleanup successful" << std::endl;

    return 0;
}

类：GalbotMotion

获取实例并初始化（get_instance && init）

适用场景：获取运动规划模块单例并初始化。必须在使用运动规划功能前调用。

examples/cpp/galbot_motion/src/get_instance_init_example.cpp

#include <iostream>
#include <set>
#include <string>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (planner.init()) {
        std::cout << "Planner initialized successfully!" << std::endl;
    } else {
        std::cerr << "Planner initialization failed!" << std::endl;
        return -1;
    }
    
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // You can still manage the robot lifecycle through GalbotRobot
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

正运动学（使用当前状态或指定 RobotStates）（forward_kinematics）

适用场景：根据给定的关节角度计算末端执行器的位姿。用于机械臂任务验证和状态分析。

examples/cpp/galbot_motion/src/forward_kinematics_example.cpp

#include <algorithm>
#include <chrono>
#include <iostream>
#include <map>
#include <string>
#include <thread>
#include <tuple>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper print function
void print_pose(const std::string& label, const std::tuple<MotionStatus, std::vector<double>>& res,
                GalbotMotion& planner) {
  std::cout << "[" << label << "] Status: " << planner.status_to_string(std::get<0>(res)) << std::endl;

  if (std::get<0>(res) == MotionStatus::SUCCESS) {
    std::cout << "End-effector pose: ";
    for (double v : std::get<1>(res)) {
      std::cout << v << " ";
    }
    std::cout << "\n" << std::endl;
  } else {
    std::cout << "Calculation failed!" << std::endl;
  }
}

int main() {
  auto& planner = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (planner.init()) {
    std::cout << "Planner initialized successfully!" << std::endl;
  } else {
    std::cerr << "Planner initialization failed!" << std::endl;
    return -1;
  }

  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
  bool navigation_ready = navigation.init();
  if (navigation_ready) {
    std::cout << "Navigation initialized successfully!" << std::endl;
  } else {
    std::cerr << "Navigation initialization failed! World/map-frame FK calls below will be skipped." << std::endl;
  }

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(3000));

  std::map<std::string, std::vector<double>> chain_joints = {
      {"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
      {"head", {0.0000, 0.0}},
      {"left_arm", {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
      {"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}};

  std::vector<double> whole_body_joint;
  std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
  for (const auto& key : keys) {
    whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
  }

  std::vector<double> base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
  std::string end_link = "left_arm_end_effector_mount_link";
  std::string reference_frame = "base_link";

  // --- test 1: defaultparameters (current status) ---
  try {
    std::cout << ">> Executing: Basic forward kinematics..." << std::endl;
    auto res1 = planner.forward_kinematics(end_link, reference_frame);
    print_pose("Basic version", res1, planner);
  } catch (const std::exception& e) {
    std::cerr << "❌ Basic version exception: " << e.what() << std::endl;
  }

  // --- test 2: jointstatus + parameters ---
  try {
    std::cout << ">> Executing: Forward kinematics with custom joints..." << std::endl;

    std::unordered_map<std::string, std::vector<double>> custom_joint_state = {{"left_arm", chain_joints["left_arm"]}};
    auto custom_param_ptr = std::make_shared<Parameter>();
    auto res2 = planner.forward_kinematics(end_link, reference_frame, custom_joint_state, custom_param_ptr);

    print_pose("Custom parameters", res2, planner);

    if (navigation_ready) {
      auto res2_world = planner.forward_kinematics(end_link, "world", custom_joint_state, custom_param_ptr);
      print_pose("Custom joints in world frame", res2_world, planner);
    } else {
      std::cout << "[Custom joints in world frame] skipped: navigation not initialized." << std::endl;
    }
  } catch (const std::exception& e) {
    std::cerr << "❌ Custom-parameter exception: " << e.what() << std::endl;
  }

  // --- test 3: RobotStates forward kinematics(current status, planning)---
  try {
    std::cout << ">> Executing: Forward kinematics based on RobotStates..." << std::endl;

    RobotStates current_state = planner.get_robot_states();
    if (current_state.whole_body_joint.empty()) {
      std::cerr << "❌ RobotStates-based: Unable to get current body joint states; ensure WBC/sensors are ready." << std::endl;
    } else {
      auto ref_robot_state_ptr = std::make_shared<RobotStates>(std::move(current_state));
      auto res3 = planner.forward_kinematics_by_state(end_link, ref_robot_state_ptr, reference_frame,
                                                      std::make_shared<Parameter>());
      print_pose("Based on RobotStates", res3, planner);
    }
  } catch (const std::exception& e) {
    std::cerr << "❌ RobotStates-based exception: " << e.what() << std::endl;
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

获取连杆名称（get_link_names）

适用场景：获取机器人模型中所有连杆的名称列表，用于碰撞检测和运动规划调试。

examples/cpp/galbot_motion/src/get_link_names_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_link_names(const std::vector<std::string>& link_names, const std::string& title) {
  std::cout << title << " (total " << link_names.size() << " items):" << std::endl;
  for (size_t i = 0; i < link_names.size(); ++i) {
    std::cout << "  " << (i + 1) << ". " << link_names[i] << std::endl;
  }
}

int main() {
  // Get object instance
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize system
  if (!motion.init()) {
    std::cerr << "GalbotMotion initialization failed!" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot initialization failed!" << std::endl;
    return -1;
  }

  std::cout << "System initialized successfully!" << std::endl;

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

  try {
    // Get all link names
    std::vector<std::string> all_link_names = motion.get_link_names(false);
    print_link_names(all_link_names, "\nAll link names");

    // getend effectorexecute link
    std::vector<std::string> ee_link_names = motion.get_link_names(true);
    print_link_names(ee_link_names, "\nEnd-effector link names");
  } catch (const std::exception& e) {
    std::cerr << "Get link name exception: " << e.what() << std::endl;
  }

  // Exit system and release SDK resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

逆运动学（基础与基于 RobotStates）(inverse_kinematics)

适用场景：根据期望的末端位姿求解所需的关节角度。是机械臂抓取等操作的核心步骤。

examples/cpp/galbot_motion/src/inverse_kinematics_example.cpp

#include <algorithm>
#include <chrono>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper function: print inverse kinematics result
void print_ik_result(const std::string& label,
                     const std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<double>>>& res,
                     GalbotMotion& planner) {
  auto status = std::get<0>(res);
  auto joint_map = std::get<1>(res);

  std::cout << "[" << label << "] Status feedback: " << planner.status_to_string(status) << std::endl;

  if (status == MotionStatus::SUCCESS) {
    std::cout << "✅ IK computation succeeded! Joint angles obtained:" << std::endl;
    for (const auto& [name, joints] : joint_map) {
      std::cout << "  - Chain [" << name << "]: ";
      for (double v : joints)
        std::cout << v << " ";
      std::cout << std::endl;
    }
  } else {
    std::cout << "❌ inverse kinematics failed, checkinput targetpose." << std::endl;
  }
  std::cout << "---------------------------------------------------" << std::endl;
}

int main() {
  auto& planner = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (planner.init()) {
    std::cout << "Planner initialized successfully!" << std::endl;
  } else {
    std::cerr << "Planner initialization failed!" << std::endl;
    return -1;
  }

  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // Joint state definition
  std::unordered_map<std::string, std::vector<double>> chain_joints = {
      {"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
      {"head", {0.0000, 0.0}},
      {"left_arm", {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
      {"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}};

  // Target pose definition (x, y, z, qx, qy, qz, qw)
  std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
      {"left_arm", {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}},
      {"right_arm", {0.1267, -0.2345, 0.7358, -0.0225, 0.0126, -0.0343, 0.9991}}};

  // Whole-body joint vector concatenation (Leg -> Head -> Left Arm -> Right Arm)
  std::vector<double> whole_body_joint;
  std::vector<std::string> key_order = {"leg", "head", "left_arm", "right_arm"};
  for (const auto& key : key_order) {
    whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
  }

  // General configuration
  std::string reference_frame = "base_link";
  std::string target_frame = "EndEffector";
  std::string target_chain = "left_arm";
  auto params = std::make_shared<Parameter>();

  // Scenario 1: Single-chain inverse kinematics
  try {
    std::cout << ">> Running Scenario 1: Single-chain IK test..." << std::endl;
    std::vector<std::string> one_chain = {target_chain};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain],  // 1. target_pose
                                          one_chain                           // 2. chain_names
                                          // target_frame,                      // 3. target_frame
                                          // reference_frame,                   // 4. reference_frame
                                          // {},                                // 5. initial_joint_positions (empty)
                                          // false,                             // 6. enable_collision_check (bool)
                                          // params                             // 7. params (shared_ptr)
    );
    print_ik_result("Single-chain inverse kinematics", res, planner);
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
  } catch (const std::exception& e) {
    std::cerr << "Scenario 1 exception: " << e.what() << std::endl;
  }

  // Scenario 2: Arm chain + torso inverse kinematics
  try {
    std::cout << ">> Running Scenario 2: Arm chain + torso IK test..." << std::endl;
    std::vector<std::string> chain_with_torso = {target_chain, "torso"};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], chain_with_torso, target_frame,
                                          reference_frame, {}, false, params);
    print_ik_result("Arm + torso inverse kinematics", res, planner);
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
  } catch (const std::exception& e) {
    std::cerr << "Scenario 2 exception: " << e.what() << std::endl;
  }

  // Scenario 3: invalid chain combination
  try {
    std::cout << ">> Running Scenario 3: Invalid chain-combination test..." << std::endl;
    std::vector<std::string> error_chains = {target_chain, "torso", "head"};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], error_chains, target_frame,
                                          reference_frame, {}, false, params);
    print_ik_result("Invalid chain combination detection", res, planner);
  } catch (const std::exception& e) {
    std::cerr << "Scenario 3 exception: " << e.what() << std::endl;
  }

  // Scenario 4: use reference joints (initial_joint_positions can specify chain joints as IK references; unspecified chain joints are filled with whole-body joints)
  try {
    std::cout << ">> Running Scenario 4: IK test with initial reference values..." << std::endl;
    std::vector<std::string> one_chain = {target_chain};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], one_chain, target_frame, reference_frame,
                                          chain_joints, false, params);
    print_ik_result("Inverse kinematics with reference values", res, planner);
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
  } catch (const std::exception& e) {
    std::cerr << "Scenario 4 exception: " << e.what() << std::endl;
  }

  // scenario 5: RobotStates inverse kinematics
  try {
    std::cout << ">> Running Scenario 5: IK test based on RobotStates..." << std::endl;

    // Construct RobotStates smart pointer
    auto ref_state = std::make_shared<RobotStates>();
    ref_state->chain_name = target_chain;
    ref_state->whole_body_joint = whole_body_joint;
    ref_state->base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

    std::vector<std::string> one_chain = {target_chain};

    auto res = planner.inverse_kinematics_by_state(chain_pose_baselink[target_chain],  // 1. target_pose
                                                   one_chain,                          // 2. chain_names
                                                   target_frame,                       // 3. target_frame
                                                   reference_frame,                    // 4. reference_frame
                                                   ref_state,  // 5. reference_robot_states (shared_ptr)
                                                   false,      // 6. enable_collision_check (bool)
                                                   params      // 7. params (shared_ptr)
    );
    print_ik_result("RobotStates inverse kinematics", res, planner);
  } catch (const std::exception& e) {
    std::cerr << "Scenario 5 exception: " << e.what() << std::endl;
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released." << std::endl;

  return 0;
}

获取与设置末端位姿(get_set_end_effort_pos)

适用场景：直接获取当前末端位姿，或将末端移动到指定位姿。集成了逆运动学计算和执行。

examples/cpp/galbot_motion/src/get_set_end_effort_pos_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper function: print pose information
void print_pose_info(const std::string& label, const std::vector<double>& pose) {
    if (pose.size() == 7) {
        std::cout << "[" << label << "] Pose: "
                  << "pos(" << pose[0] << ", " << pose[1] << ", " << pose[2] << "), "
                  << "ori(" << pose[3] << ", " << pose[4] << ", " << pose[5] << ", " << pose[6] << ")" 
                  << std::endl;
    }
}

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
        {"leg",       {0.0596, -0.0000, 1.0327, 0.5000, 0.5003, 0.4997, 0.5000}},
        {"head",      {0.0599, 0.0002, 1.4098, -0.7072, 0.0037, 0.0037, 0.7069}},
        {"left_arm",  {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}},
        {"right_arm", {0.097768, -0.226021, 0.8, -0.0117403, -0.0098713, 0.0157502, 0.999758}}
    };

    std::string reference_frame = "base_link";
    std::string target_frame = "EndEffector";
    std::string target_chain = "left_arm";
    auto custom_param = std::make_shared<Parameter>();

    // --- Scenario 1: Get end-effector pose (basic version) ---
    try {
        std::cout << ">> Scenario 1: Getting the basic end-effector pose..." << std::endl;
        std::string end_ee_link = "left_arm_end_effector_mount_link";

        auto res = planner.get_end_effector_pose(end_ee_link, reference_frame);
        
        MotionStatus status = std::get<0>(res);
        std::vector<double> pose = std::get<1>(res);

        std::cout << "Execution status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            print_pose_info("Basic version", pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) {
        std::cerr << "❌ Scenario 1 exception: " << e.what() << std::endl;
    }

    // --- Scenario 2: Get end-effector pose by specified chain name + custom frame ---
    try {
        std::cout << ">> Scenario 2: Getting pose by specified chain name..." << std::endl;

        auto res = planner.get_end_effector_pose_on_chain(target_chain, target_frame, reference_frame);
        
        MotionStatus status = std::get<0>(res);
        std::vector<double> pose = std::get<1>(res);

        std::cout << "Execution status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            print_pose_info("Specified chain name version", pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) {
        std::cerr << "❌ Scenario 2 exception: " << e.what() << std::endl;
    }

    // --- Scenario 3: Set end-effector pose ---
    try {
        std::cout << ">> Scenario 3: Setting end-effector pose..." << std::endl;
        
        std::string ee_frame = "left_arm"; 
        std::vector<double> target_pose = chain_pose_baselink[ee_frame];

        MotionStatus status = planner.set_end_effector_pose(
            target_pose,        // 1
            ee_frame,           // 2
            reference_frame,    // 3
            nullptr,            // 4. Important: pass nullptr if no specific reference state is used
            false,              // 5. enable_collision_check
            true,               // 6. is_blocking
            5.0,                // 7. timeout
            custom_param        // 8. params
        );

        std::cout << "Set status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            std::cout << "✅ Command sent successfully (blocking wait mode)" << std::endl;
        }
    } catch (const std::exception& e) {
        std::cerr << "❌ Pose-setting exception: " << e.what() << std::endl;
    }

    // --- Scenario 4: Get end-effector pose again after execution ---
    try {
        std::cout << ">> Scenario 4: Getting the basic end-effector pose..." << std::endl;
        std::string end_ee_link = "left_arm_end_effector_mount_link";

        auto res = planner.get_end_effector_pose(end_ee_link, reference_frame);
        
        MotionStatus status = std::get<0>(res);
        std::vector<double> pose = std::get<1>(res);

        std::cout << "Execution status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            print_pose_info("Basic version", pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) {
        std::cerr << "❌ Scenario 4 exception: " << e.what() << std::endl;
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

单点运动规划（关节空间与笛卡尔空间）（single_point_planning）

适用场景：规划从当前位姿到单个目标位姿的无碰撞运动轨迹。适用于单点目标到达任务。

examples/cpp/galbot_motion/src/single_point_planning_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Define a trajectory return type to simplify the code
using TrajResult = std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<std::vector<double>>>>;

/**
 * Helper function: print planning result
 */
void print_plan_info(const std::string& label, const TrajResult& res, const std::string& chain_name, GalbotMotion& planner) {
    auto status = std::get<0>(res);
    auto traj_map = std::get<1>(res);

    std::cout << "[" << label << "] Status: " << planner.status_to_string(status) << std::endl;
    if (status == MotionStatus::SUCCESS) {
        if (traj_map.count(chain_name) && !traj_map[chain_name].empty()) {
            std::cout << "✅ Planning succeeded: trajectory points = " << traj_map[chain_name].size() << std::endl;
        } else {
            std::cout << "⚠️ Status is SUCCESS but trajectory is empty (possibly already at target)" << std::endl;
        }
    } else {
        std::cout << "❌ Planning failed" << std::endl;
    }
    std::cout << "---------------------------------------" << std::endl;
}

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!planner.init()) {
        std::cerr << "GalbotMotion init FAILED" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot init FAILED" << std::endl;
        return -1;
    }
    
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg",       {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
        {"head",      {0.0000, 0.0}},
        {"left_arm",  {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
        {"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}
    };

    std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
        {"leg",       {0.0596, -0.0000, 1.0327, 0.5000, 0.5003, 0.4997, 0.5000}},
        {"head",      {0.0599, 0.0002, 1.4098, -0.7072, 0.0037, 0.0037, 0.7069}},
        {"left_arm",  {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}},
        {"right_arm", {0.1267, -0.2345, 0.7358, -0.0225, 0.0126, -0.0343, 0.9991}}
    };

    auto params = std::make_shared<Parameter>();
    std::string target_chain = "left_arm";

    // NOTE:
    // - GalbotMotion does NOT provide real-time obstacle perception / automatic environment updates today.
    // - When enable_collision_check=true, collision checking uses self-collision + objects you load manually via
    //   add_obstacle/attach_target_object (including point clouds if you load them explicitly).

    // Scenario 1: joint-space planning, target type = joint state
    try {
        std::cout << ">> Scenario 1: joint-space planning (joint target)..." << std::endl;

        // Construct target joint state
        auto target_joint = std::make_shared<JointStates>();
        target_joint->chain_name = target_chain;
        target_joint->joint_positions = chain_joints[target_chain];

        auto res = planner.motion_plan(
            target_joint,   // 1. target
            nullptr,        // 2. start (nullptr means start from current state)
            nullptr,        // 3. reference_robot_states
            false,          // 4. enable_collision_check
            params          // 5. params
        );

        print_plan_info("Joint-space planning (joint target)", res, target_chain, planner);
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    // Scenario 2: joint-space planning, target type = end-effector pose (Cartesian)
    try {
        std::cout << ">> Scenario 2: joint-space planning (pose target)..." << std::endl;

        // Construct target pose state
        auto target_pose = std::make_shared<PoseState>();
        target_pose->chain_name = target_chain;
        target_pose->frame_id = "EndEffector";
        target_pose->reference_frame = "base_link";
        target_pose->pose = chain_pose_baselink[target_chain];

        auto res = planner.motion_plan(
            target_pose,    // 1. target
            nullptr,        // 2. start
            nullptr,        // 3. reference_robot_states
            false,          // 4. enable_collision_check
            params          // 5. params
        );

        print_plan_info("Joint-space planning (pose target)", res, target_chain, planner);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    // Scenario 3: joint-space planning with an explicit start state
    try {
        std::cout << ">> Scenario 3: joint-space planning (explicit start)..." << std::endl;

        auto target_joint = std::make_shared<JointStates>();
        target_joint->chain_name = target_chain;
        target_joint->joint_positions = chain_joints[target_chain];

        auto start_joint = std::make_shared<JointStates>();
        start_joint->chain_name = target_chain;
        start_joint->joint_positions = std::vector<double>(7, 0.0); // Use seven zeros as the starting point

        auto res = planner.motion_plan(
            target_joint, 
            start_joint,    // 2. Specify the start point
            nullptr, 
            false, 
            params
        );

        print_plan_info("Joint-space planning (explicit start)", res, target_chain, planner);
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

多点轨迹规划(multi_point_planning)

适用场景：规划经过多个路径点的连续运动轨迹，适用于需要经过多个中间点的复杂运动。

examples/cpp/galbot_motion/src/multi_point_planning_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>
#include <algorithm>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Trajectory return type definition
using TrajResult = std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<std::vector<double>>>>;

/**
 * Helper function: print planning result
 */
void print_multi_plan_result(const std::string& label, const TrajResult& res, const std::string& chain_name, GalbotMotion& planner) {
    auto status = std::get<0>(res);
    auto traj_map = std::get<1>(res);

    std::cout << "[" << label << "] Status feedback: " << planner.status_to_string(status) << std::endl;
    if (status == MotionStatus::SUCCESS) {
        if (traj_map.count(chain_name) && !traj_map[chain_name].empty()) {
            std::cout << "✅ Multi-point planning succeeded: total trajectory points = " << traj_map[chain_name].size() << std::endl;
        } else {
            std::cout << "⚠️ Status is SUCCESS but trajectory is empty; target may overlap current pose." << std::endl;
        }
    } else {
        std::cout << "❌ Multi-point planning failed." << std::endl;
    }
    std::cout << "---------------------------------------------------" << std::endl;
}

int main() {
    std::cout << "WARNING: The robot will move to the initial joint state. "
              << "Release the emergency stop and clear nearby obstacles." << std::endl;
    std::cout << "Continue? (y/n): ";
    std::string response;
    std::getline(std::cin, response);
    if (response != "y" && response != "Y") {
        std::cout << "Example cancelled." << std::endl;
        return 0;
    }

    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }
    
    std::this_thread::sleep_for(std::chrono::seconds(2));

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg",       {0.5, 1.5, 1.0, 0.0, 0.0}},
        {"head",      {0.0, 0.0}},
        {"left_arm",  {2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0}},
        {"right_arm", {-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
    for (const auto& key : keys) {
        whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
    }

    const ControlStatus move_status =
        robot.set_joint_positions(whole_body_joint, keys, {}, true, 0.1, 30.0);
    if (move_status != ControlStatus::SUCCESS) {
        std::cerr << "❌ Failed to move to the initial joint state." << std::endl;
        robot.request_shutdown();
        robot.wait_for_shutdown();
        robot.destroy();
        return -1;
    }
    std::cout << "✅ Moved to the initial whole-body joint state." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(3));

    auto params = std::make_shared<Parameter>();
    std::string target_chain = "left_arm";

    // NOTE: The two scenarios express the same physical waypoints as Cartesian
    // poses and joint positions, so their planning results are consistent. Calling
    // params->set_move_line(true) applies Cartesian-space line planning to both.
    // 注意：以下两个场景分别使用笛卡尔位姿和关节位置表达相同的物理路点，
    // 因此规划结果一致。调用 params->set_move_line(true) 后，二者均按
    // 笛卡尔空间直线方式规划。

    // --- Scenario 1: Multi-waypoint planning in Cartesian space (PoseState target) ---
    try {
        std::cout << ">> Running Scenario 1: Multi-waypoint planning in Cartesian space..." << std::endl;

        auto target_pose_state = std::make_shared<PoseState>();
        target_pose_state->chain_name = target_chain;

        // Construct waypoints (3 intermediate poses)
        std::vector<std::vector<double>> waypoint_poses = {
            {0.1297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968},
            {0.2297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968},
            {0.3297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968},
            {0.3297, 0.2608, 0.8417, 0.0734, 0.0209, -0.0233, 0.9968}
        };

        auto res = planner.motion_plan_multi_waypoints(
            target_pose_state,
            waypoint_poses,
            nullptr,  // start
            nullptr,  // reference_robot_states
            false,    // enable_collision_check
            params    // params
        );

        print_multi_plan_result("Cartesian multi-waypoint single-chain planning", res, target_chain, planner);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) { std::cerr << "Scenario 1 exception: " << e.what() << std::endl; }

    // --- Scenario 2: Multi-waypoint planning in joint space (JointStates target) ---
    try {
        std::cout << ">> Running Scenario 2: Multi-waypoint planning in joint space..." << std::endl;

        auto target_joint = std::make_shared<JointStates>();
        target_joint->chain_name = target_chain;

        // Construct waypoints (3 intermediate poses)
        std::vector<std::vector<double>> waypoints = {
            {2.0000, -1.5001, -0.6001, -1.7000, 0.0001, -0.7999, 0.0000},
            {1.7805, -1.4841, -0.5856, -1.6857, -0.0083, -0.5952, 0.0090},
            {1.5313, -1.4746, -0.5672, -1.5923, -0.0109, -0.4406, 0.0185},
            {1.6264, -1.4551, -0.5976, -1.9286, -0.0427, -0.1972, 0.0289}
        };

        auto res = planner.motion_plan_multi_waypoints(
            target_joint,
            waypoints,
            nullptr,
            nullptr,
            false,
            params
        );

        print_multi_plan_result("Joint-space multi-waypoint", res, target_chain, planner);
    } catch (const std::exception& e) { std::cerr << "Scenario 2 exception: " << e.what() << std::endl; }

    // 4. Clean up resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

碰撞检测(collision_detection)

适用场景：检查给定关节状态是否会发生自碰撞或与环境障碍物碰撞。用于运动规划前的可行性检查。

examples/cpp/galbot_motion/src/collision_detection_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>
#include <algorithm>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
#include "galbot_navigation.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);

    // NOTE:
    // - GalbotNavigation (galbotNav) may use real-time obstacle perception/avoidance during navigation (deployment dependent).
    // - GalbotMotion does NOT provide real-time obstacle perception today; Motion collision uses self-collision +
    //   manually loaded obstacles (add_obstacle/attach_target_object).

    if (!planner.init()) {
        std::cerr << "GalbotMotion init FAILED" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot init FAILED" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "GalbotNavigation init FAILED" << std::endl;
        return -1;
    }

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg",       {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
        {"head",      {0.0000, 0.0}},
        {"left_arm",  {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
        {"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
    for (const auto& key : keys) {
        whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
    }

    std::vector<double> base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
    std::vector<double> bad_left_arm_joint = {1.99995, -1.60004, 0.599905, -1.69994, 0, -0.799924, 0};
    auto custom_param = std::make_shared<Parameter>();

    try {
        std::cout << ">> Running collision check..." << std::endl;

        // Construct a RobotStates list for collision checking
        std::vector<std::shared_ptr<RobotStates>> check_states;

        // status 0: status (RobotStates)
        auto state0 = std::make_shared<RobotStates>();
        state0->whole_body_joint = whole_body_joint;
        state0->base_state = base_state;
        check_states.push_back(state0);

        // status 1: status (JointStates RobotStates)
        auto state1 = std::make_shared<JointStates>();
        state1->chain_name = "left_arm";
        state1->joint_positions = bad_left_arm_joint;
        check_states.push_back(state1);

        // Call collision detection interface
        auto res = planner.check_collision(check_states, true, custom_param);

        MotionStatus status = std::get<0>(res);
        std::vector<bool> collision_results = std::get<1>(res);

        std::cout << "Status: " << planner.status_to_string(status) << std::endl;

        if (status == MotionStatus::SUCCESS) {
            std::cout << "OK: collision check finished (false=no collision, true=collision):" << std::endl;
            for (size_t i = 0; i < collision_results.size(); ++i) {
                std::cout << "  - status [" << i << "]: " 
                          << (collision_results[i] ? "COLLISION" : "NO COLLISION") 
                          << std::endl;
            }
        } else {
            std::cerr << "ERROR: collision check returned failure." << std::endl;
        }

    } catch (const std::exception& e) {
        std::cerr << "ERROR: collision check exception: " << e.what() << std::endl;
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

附加工具(attach_tool)

适用场景：在机器人末端添加工具（如夹具、吸盘等），更新运动规划模型，以便考虑工具质量和碰撞体积。

examples/cpp/galbot_motion/src/attach_tool_example.cpp

#include <iostream>
#include <string>
#include <thread>
#include <chrono>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // --- Execute tool-attach operation ---
    try {
        std::string chain_name = "left_arm";
        std::string tool_name = "suction_cup";

        MotionStatus status = planner.attach_tool(chain_name, tool_name);

        std::cout << "Execution status feedback: " << planner.status_to_string(status) << std::endl;

        if (status == MotionStatus::SUCCESS) {
            std::cout << "✅ Tool attached successfully: " << tool_name << std::endl;

            // Clean up the attached tool model to avoid affecting subsequent examples.
            status = planner.detach_tool(chain_name);
            std::cout << "Cleanup status feedback: " << planner.status_to_string(status) << std::endl;

            if (status == MotionStatus::SUCCESS) {
                std::cout << "✅ Tool detached successfully." << std::endl;
            } else {
                std::cerr << "❌ Tool cleanup failed." << std::endl;
            }
        } else {
            std::cerr << "❌ Tool attachment failed. Please check whether the tool name is defined in the configuration file." << std::endl;
        }

    } catch (const std::exception& e) {
        std::cerr << "❌ Exception occurred during runtime: " << e.what() << std::endl;
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

卸载工具(detach_tool)

适用场景：从机器人末端移除已附加的工具，恢复原始机器人模型。

examples/cpp/galbot_motion/src/detach_tool_example.cpp

#include <iostream>
#include <string>
#include <thread>
#include <chrono>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // --- Execute tool-detach operation ---
    try {
        std::string chain_name = "left_arm";
        MotionStatus status = planner.detach_tool(chain_name);

        std::cout << "Execution status feedback: " << planner.status_to_string(status) << std::endl;

        if (status == MotionStatus::SUCCESS) {
            std::cout << "✅ Tool detached successfully." << std::endl;
        } else {
            std::cerr << "❌ Tool detachment failed." << std::endl;
        }

    } catch (const std::exception& e) {
        std::cerr << "❌ Exception occurred during runtime: " << e.what() << std::endl;
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

加载/移除环境碰撞体(env_collider_operation)

适用场景：向运动规划环境中添加障碍物或移除已添加的障碍物，使运动规划考虑环境障碍。

examples/cpp/galbot_motion/src/env_collider_operation_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <array>
#include <thread>
#include <chrono>
#include <memory>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

/*
    NOTE:
    - GalbotMotion does NOT provide real-time obstacle perception / automatic environment updates today.
    - To make Motion collision checking consider environmental obstacles, you must load them manually
        (e.g., box/mesh/point_cloud via add_obstacle/attach_target_object).
    - Real-time perception / navigation-style obstacle updates in Motion is a planned future feature.
*/

int main() {
    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!planner.init()) {
        std::cerr << "GalbotMotion init FAILED" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot init FAILED" << std::endl;
        return -1;
    }
    
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // Scenario 1: add a Box collision object into Motion environment.
    try {
        std::cout << ">> Scenario 1: add obstacle..." << std::endl;

        std::string obstacle_id = "box_test_1";
        std::string obj_type = "box";
        std::vector<double> obj_pose = {1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        std::array<double, 3> obj_scale = {1.0, 1.0, 1.0};
        std::string target_frame = "world";

        MotionStatus status = planner.add_obstacle(
            obstacle_id,      // 1. ID
            obj_type,         // 2. Type
            obj_pose,         // 3. Pose
            obj_scale,        // 4. Scale (array)
            "",               // 5. key
            target_frame,     // 6. target_frame
            "",               // 7. ee_frame
            {},               // 8. ref_joints
            {},               // 9. ref_base
            {},               // 10. ignore_links
            0.0,              // 11. safe_margin
            0.0               // 12. resolution
        );

        std::cout << "Status: " << planner.status_to_string(status) << std::endl;
        
        planner.clear_obstacle();
        
        if (status == MotionStatus::SUCCESS) {
            std::cout << "OK: obstacle added" << std::endl;
        }
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    // Scenario 2: add a duplicate ID (expected to fail).
    try {
        std::cout << "\n>> Scenario 2: test duplicate ID..." << std::endl;

        std::string obstacle_id = "box_test_2";
        std::string obj_type = "box";
        std::vector<double> obj_pose = {1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        std::array<double, 3> obj_scale = {1.0, 1.0, 1.0};
        std::string target_frame = "world";

        // First addition
        planner.add_obstacle(obstacle_id, obj_type, obj_pose, obj_scale, "", target_frame, "", {}, {}, {}, 0.0, 0.0);

        // Second addition with same ID
        MotionStatus status = planner.add_obstacle(obstacle_id, obj_type, obj_pose, obj_scale, "", target_frame, "", {}, {}, {}, 0.0, 0.0);

        std::cout << "Status: " << planner.status_to_string(status) << std::endl;
        
        planner.clear_obstacle();

        if (status == MotionStatus::FAULT) {
            std::cout << "OK: duplicate ID rejected (expected)" << std::endl;
        }
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

详细碰撞检测（check_collision_detail）

适用场景：查看详细碰撞结果，确定发生碰撞的机器人连杆或环境物体。

examples/cpp/galbot_motion/src/check_collision_detail_example.cpp

#include <iostream>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_collision_details(GalbotMotion& motion, const std::string& title, MotionStatus status,
                             const std::vector<CollisionInfo>& collision_infos) {
  std::cout << title << " status: " << motion.status_to_string(status) << std::endl;
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (collision_infos.empty()) {
    std::cout << "  no collision pair returned" << std::endl;
    return;
  }
  for (size_t i = 0; i < collision_infos.size(); ++i) {
    const auto& info = collision_infos[i];
    std::cout << "  - pair [" << i << "]: " << (info.is_collision ? "COLLISION" : "NO COLLISION") << ", "
              << info.link1 << " <-> " << info.link2 << ", distance: " << info.distance
              << ", type: " << info.collision_type << std::endl;
  }
}

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  auto& navigation = GalbotNavigation::get_instance(MachineType::G1);

  if (!motion.init()) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "GalbotNavigation init FAILED" << std::endl;
    return -1;
  }

  auto params = std::make_shared<Parameter>();
  params->set_timeout(5.0);

  try {
    std::cout << ">> Detailed collision check: current robot state" << std::endl;
    auto current_result = motion.check_collision_detail(std::vector<std::shared_ptr<RobotStates>>{}, false, false, params);
    print_collision_details(motion, "current robot state", std::get<0>(current_result), std::get<1>(current_result));

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
        {"head", {0.0000, 0.0}},
        {"left_arm", {2.0, -1.6, -0.6, -1.7, 0.0, -0.8, 0.0}},
        {"right_arm", {-2.0, 1.6, 0.6, 1.7, 0.0, 0.8, 0.0}},
    };
    std::vector<double> whole_body_joint;
    for (const auto& key : {"leg", "head", "left_arm", "right_arm"}) {
      whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
    }

    std::vector<std::shared_ptr<RobotStates>> robot_states;
    auto whole_body_state = std::make_shared<RobotStates>();
    whole_body_state->whole_body_joint = whole_body_joint;
    whole_body_state->base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
    robot_states.push_back(whole_body_state);

    auto left_arm_state = std::make_shared<JointStates>();
    left_arm_state->chain_name = "left_arm";
    left_arm_state->joint_positions = {2.0, -1.6, 0.6, -1.7, 0.0, -0.8, 0.0};
    robot_states.push_back(left_arm_state);

    std::cout << ">> Detailed collision check: explicit robot states" << std::endl;
    auto explicit_result = motion.check_collision_detail(robot_states, false, true, params);
    print_collision_details(motion, "explicit robot states", std::get<0>(explicit_result), std::get<1>(explicit_result));
  } catch (const std::exception& e) {
    std::cerr << "ERROR: check_collision_detail exception: " << e.what() << std::endl;
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

组合运动规划（combine_plan）

适用场景：将多个运动链目标组合为一个协调运动规划请求。

examples/cpp/galbot_motion/src/combine_plan_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
  std::unordered_map<std::string, std::vector<double>> poses;
};

struct ExampleInputs {
  std::vector<PlanRequest> plan_reqs;
  Parameter params;
};

struct StartJointState {
  std::vector<double> leg;
  std::vector<double> head;
  std::vector<double> left_arm;
  std::vector<double> right_arm;
};

const StartJointState kCombinePlanStartState = {
    {0.499191, 1.49907, 1.00046, 0.0, 0.0},
    {0.0, 0.00989756},
    {1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
    {-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0},
};

constexpr double kStartMoveSpeedRadS = 0.12;
constexpr double kStartMoveTimeoutS = 30.0;
const std::vector<std::string> kUpperBodyGroups = {"head", "left_arm", "right_arm"};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

std::vector<double> pose_to_vector(const Pose& pose) {
  return {pose.position.x,    pose.position.y,    pose.position.z,   pose.orientation.x,
          pose.orientation.y, pose.orientation.z, pose.orientation.w};
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

std::vector<double> make_upper_body_start_positions(const StartJointState& start_state) {
  std::vector<double> positions;
  positions.reserve(start_state.head.size() + start_state.left_arm.size() + start_state.right_arm.size());
  positions.insert(positions.end(), start_state.head.begin(), start_state.head.end());
  positions.insert(positions.end(), start_state.left_arm.begin(), start_state.left_arm.end());
  positions.insert(positions.end(), start_state.right_arm.begin(), start_state.right_arm.end());
  return positions;
}

bool move_robot_to_start_state(GalbotRobot& robot) {
  std::cout << "Move robot to combine_plan start state." << std::endl;
  std::cout << "  leg start joints: ";
  print_vector(kCombinePlanStartState.leg);
  std::cout << std::endl;

  // const ControlStatus controller_status = robot.switch_controller(G1ControllerName::LEG_PVT_CTRL);
  // print_api_status("GalbotRobot::switch_controller(leg_pvt_ctrl)", controller_status);
  // if (controller_status != ControlStatus::SUCCESS) {
  //   return false;
  // }
  // std::this_thread::sleep_for(std::chrono::milliseconds(500));
  const ControlStatus leg_status =
      robot.set_joint_positions(kCombinePlanStartState.leg, {"leg"}, {}, true, kStartMoveSpeedRadS, kStartMoveTimeoutS);
  print_api_status("GalbotRobot::set_joint_positions(leg start)", leg_status);
  if (leg_status != ControlStatus::SUCCESS) {
    return false;
  }

  const std::vector<double> upper_body_start = make_upper_body_start_positions(kCombinePlanStartState);
  std::cout << "  upper-body start joints [head, left_arm, right_arm]: ";
  print_vector(upper_body_start);
  std::cout << std::endl;

  const ControlStatus upper_body_status =
      robot.set_joint_positions(upper_body_start, kUpperBodyGroups, {}, true, kStartMoveSpeedRadS, kStartMoveTimeoutS);
  print_api_status("GalbotRobot::set_joint_positions(head+arms start)", upper_body_status);
  return upper_body_status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_joint_target(const std::string& chain, const std::vector<double>& joint_positions,
                                        const std::vector<std::string>& joint_names = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kJoint;
  target.joint.chain_name = chain;
  target.joint.joint_positions = joint_positions;
  target.joint.joint_names = joint_names;
  return target;
}

MotionPlanChainTarget make_cartesian_target(const std::string& chain, const std::vector<double>& pose,
                                            const std::vector<std::string>& assist_chains = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kCartesian;
  target.cart.chain_name = chain;
  target.cart.frame_id = "EndEffector";
  target.cart.reference_frame = "base_link";
  target.cart.pose = Pose(pose);
  target.cart.assist_chains.insert(assist_chains.begin(), assist_chains.end());
  return target;
}


bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

bool capture_current_pose(GalbotMotion& motion, GalbotRobot& robot, const std::string& chain,
                          CurrentRobotState& current) {
  if (!capture_current_joints(motion, robot, {chain}, current)) {
    return false;
  }

  auto [status, pose] = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link");
  print_api_status("get_end_effector_pose_on_chain(" + chain + ")", status, motion);
  if (status != MotionStatus::SUCCESS || pose.size() != 7) {
    std::cerr << "Failed to get current pose for chain: " << chain << std::endl;
    return false;
  }

  current.poses[chain] = pose;
  std::cout << "Current " << chain << " pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(pose);
  std::cout << std::endl;
  return true;
}

ExampleInputs make_example_inputs(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");

  PlanRequest request0;
  request0.plan_type = MotionPlanType::MOTION_PLAN;
  request0.enforce_pass = true;
  request0.target = {
      {make_joint_target("left_arm", {1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_cartesian_target("right_arm", {0.22666, -0.23435, 0.73569, 0.0, 0.0, 0.0, 1.0}, {"torso"})},
      {make_joint_target("left_arm", {1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names)},
      {make_joint_target("left_arm", {1.99995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_cartesian_target("right_arm", {0.12666, -0.23435, 0.73569, 0.0, 0.0, 0.0, 1.0}, {"torso"})},
  };

  PlanRequest request1;
  request1.plan_type = MotionPlanType::MOVE_LINE;
  request1.enforce_pass = true;
  request1.target = {
      {make_cartesian_target("left_arm", {0.42666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0}, {"torso"}),
       make_cartesian_target("right_arm", {0.422666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0}, {"torso"})},
      {make_cartesian_target("left_arm", {0.12666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0}, {"torso"})},
      {make_cartesian_target("right_arm", {0.12666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0}, {"torso"})},
  };

  PlanRequest request2;
  request2.plan_type = MotionPlanType::MOTION_PLAN;
  request2.enforce_pass = true;
  request2.target = {
      {make_joint_target("left_arm", {1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names)},
      {make_joint_target("right_arm", {-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names)},
      {make_joint_target("torso", {0.0, 0.0}, {"leg_joint4", "leg_joint5"})},
  };

  inputs.plan_reqs = {request0, request1, request2};

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(true);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

std::string plan_type_to_string(MotionPlanType plan_type) {
  switch (plan_type) {
    case MotionPlanType::MOTION_PLAN:
      return "MOTION_PLAN";
    case MotionPlanType::TRAJ_PLAN:
      return "TRAJ_PLAN";
    case MotionPlanType::MOVE_LINE:
      return "MOVE_LINE";
    default:
      return "UNKNOWN";
  }
}

void print_joint_names(const std::vector<std::string>& joint_names) {
  if (joint_names.empty()) {
    return;
  }
  std::cout << " joint_names=[";
  for (size_t i = 0; i < joint_names.size(); ++i) {
    std::cout << joint_names[i];
    if (i + 1 < joint_names.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

void print_plan_requests(const std::vector<PlanRequest>& plan_requests) {
  std::cout << "combine_plan plan requests: " << plan_requests.size() << std::endl;
  for (size_t request_index = 0; request_index < plan_requests.size(); ++request_index) {
    const auto& request = plan_requests[request_index];
    std::cout << "  request[" << request_index << "] type=" << plan_type_to_string(request.plan_type)
              << " waypoints=" << request.target.size() << std::endl;
    for (size_t waypoint_index = 0; waypoint_index < request.target.size(); ++waypoint_index) {
      std::cout << "    waypoint[" << waypoint_index << "] targets: " << request.target[waypoint_index].size()
                << std::endl;
      for (const auto& target : request.target[waypoint_index]) {
        if (target.mode == MotionPlanTargetMode::kJoint) {
          std::cout << "      " << target.chain_name << " JOINT q=";
          print_vector(target.joint.joint_positions);
          print_joint_names(target.joint.joint_names);
          std::cout << std::endl;
        } else {
          std::cout << "      " << target.chain_name << " CART pose=";
          print_vector(pose_to_vector(target.cart.pose));
          std::cout << " frame_id=" << target.cart.frame_id
                    << " reference_frame=" << target.cart.reference_frame;
          if (!target.cart.assist_chains.empty()) {
            std::cout << " assist_chains=[";
            size_t assist_chain_index = 0;
            for (const auto& assist_chain : target.cart.assist_chains) {
              std::cout << assist_chain;
              if (++assist_chain_index < target.cart.assist_chains.size()) {
                std::cout << ", ";
              }
            }
            std::cout << "]";
          }
          std::cout << std::endl;
        }
      }
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_start_state(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_pose(motion, robot, "left_arm", current) ||
      !capture_current_pose(motion, robot, "right_arm", current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs(current);
  print_plan_requests(inputs.plan_reqs);

  std::cout << "Immediate execution is enabled; plan requests are loaded from combine_file_example_plan_reqs.json reference values."
            << std::endl;

  auto result = motion.combine_plan(inputs.plan_reqs, inputs.params);
  print_traj_result("combine_plan", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

获取运动链关节名称（get_chain_joint_names）

适用场景：构造关节状态或目标前，查询运动链中按顺序排列的关节名称。

examples/cpp/galbot_motion/src/get_chain_joint_names_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

struct ExampleInputs {
  std::vector<std::string> chain_names;
};

ExampleInputs make_example_inputs() {
  ExampleInputs inputs;

  // get_chain_joint_names(chain_name) input.
  inputs.chain_names = {
      "left_arm",
      "right_arm",
      "leg",
      "head",
      "invalid_chain",
  };

  return inputs;
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize SDK interfaces directly in the example so users can see the required order.
  if (!motion.init()) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  const ExampleInputs inputs = make_example_inputs();
  for (const auto& chain : inputs.chain_names) {
    // This is the API being demonstrated: query joint names by chain name.
    const auto joint_names = motion.get_chain_joint_names(chain);
    std::cout << "get_chain_joint_names(" << chain << "): ";
    if (joint_names.empty()) {
      std::cout << "<empty>";
    } else {
      for (const auto& name : joint_names) {
        std::cout << name << " ";
      }
    }
    std::cout << std::endl;
  }

  // Clean shutdown releases SDK resources before the process exits.
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

获取雅可比矩阵（get_jacobian）

适用场景：计算末端执行器雅可比矩阵，用于微分运动学和速度分析。

examples/cpp/galbot_motion/src/get_jacobian_example.cpp

#include <chrono>
#include <iomanip>
#include <iostream>
#include <map>
#include <set>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    if (i > 0) {
      std::cout << ", ";
    }
    std::cout << values[i];
  }
  std::cout << "]";
}

void print_joint_state(const std::unordered_map<std::string, std::vector<double>>& joint_state) {
  std::cout << "{" << std::endl;
  for (const auto& pair : joint_state) {
    std::cout << "    " << pair.first << ": ";
    print_vector(pair.second);
    std::cout << std::endl;
  }
  std::cout << "  }" << std::endl;
}

void print_get_jacobian_params(const std::string& chain_name,
                               const std::string& target_frame,
                               const std::string& reference_frame,
                               const std::unordered_map<std::string, std::vector<double>>& joint_state) {
  std::cout << "get_jacobian parameters:" << std::endl;
  std::cout << "  chain_name: " << chain_name << std::endl;
  std::cout << "  target_frame: " << target_frame << std::endl;
  std::cout << "  reference_frame: " << reference_frame << std::endl;
  std::cout << "  joint_state: ";
  print_joint_state(joint_state);
}

void print_jacobian(const std::string& label,
                    const std::tuple<MotionStatus, std::vector<std::vector<double>>>& result,
                    GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& matrix = std::get<1>(result);

  std::cout << "[" << label << "] Status: " << motion.status_to_string(status) << std::endl;
  if (status != MotionStatus::SUCCESS || matrix.empty()) {
    std::cout << "Jacobian computation failed or returned an empty matrix." << std::endl;
    return;
  }

  std::cout << "Jacobian matrix (" << matrix.size() << "x" << matrix[0].size() << "):" << std::endl;
  for (size_t r = 0; r < matrix.size(); ++r) {
    std::cout << "  row " << r << ": ";
    for (double value : matrix[r]) {
      std::cout << std::setw(10) << std::fixed << std::setprecision(5) << value << " ";
    }
    std::cout << std::endl;
  }
}

std::vector<double> make_fixed_joint_values(size_t dof) {
  static const std::vector<double> fixed_values = {-1.2, -0.8, -0.4, 0.0, 0.4, 0.8, 1.2};
  std::vector<double> values;
  values.reserve(dof);
  for (size_t i = 0; i < dof; ++i) {
    values.push_back(fixed_values[i % fixed_values.size()]);
  }
  return values;
}

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (!motion.init()) {
    std::cerr << "GalbotMotion initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotMotion initialized successfully" << std::endl;

  if (!robot.init()) {
    std::cerr << "GalbotRobot initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotRobot initialized successfully" << std::endl;

  std::this_thread::sleep_for(std::chrono::seconds(3));

  std::set<std::string> support_chains = motion.get_support_chains();
  std::cout << "support_chains (" << support_chains.size() << "): [";
  for (auto it = support_chains.begin(); it != support_chains.end(); ++it) {
    if (it != support_chains.begin()) {
      std::cout << ", ";
    }
    std::cout << *it;
  }
  std::cout << "]" << std::endl;
  if (support_chains.empty()) {
    support_chains = {"head", "left_arm", "right_arm", "torso", "leg"};
  }

  std::unordered_map<std::string, std::vector<double>> current_chain_joint_state;
  try {
    current_chain_joint_state = motion.get_chain_joint_state();
  } catch (const std::exception& e) {
    std::cerr << "get_chain_joint_state exception: " << e.what() << std::endl;
  }

  const std::string target_frame = "EndEffector";
  const std::string reference_frame = "base_link";

  // Known chain DOF used only when the runtime joint state is unavailable.
  const std::unordered_map<std::string, size_t> fallback_dof = {
      {"head", 2}, {"left_arm", 7}, {"right_arm", 7}, {"leg", 5}};

  for (const auto& chain_name : support_chains) {
    // Prefer the runtime joint count; fall back to a known DOF when it is unavailable.
    size_t dof = 0;
    const auto current_it = current_chain_joint_state.find(chain_name);
    if (current_it != current_chain_joint_state.end() && !current_it->second.empty()) {
      dof = current_it->second.size();
    } else {
      const auto fb = fallback_dof.find(chain_name);
      if (fb != fallback_dof.end()) {
        dof = fb->second;
      }
    }
    if (dof == 0) {
      std::cerr << "Skip chain '" << chain_name << "': unable to determine chain DOF." << std::endl;
      continue;
    }

    std::unordered_map<std::string, std::vector<double>> joint_state = {
        {chain_name, make_fixed_joint_values(dof)}};

    try {
      std::cout << "=== get_jacobian: " << chain_name << " ===" << std::endl;
      print_get_jacobian_params(chain_name, target_frame, reference_frame, joint_state);
      auto result = motion.get_jacobian(chain_name, target_frame, reference_frame, joint_state);
      print_jacobian(chain_name, result, motion);
    } catch (const std::exception& e) {
      std::cerr << chain_name << " exception: " << e.what() << std::endl;
    }
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

基于机器人状态获取雅可比矩阵（get_jacobian_by_state）

适用场景：在显式指定的机器人状态下计算雅可比矩阵，而不是使用当前状态。

examples/cpp/galbot_motion/src/get_jacobian_by_state_example.cpp

#include <chrono>
#include <iomanip>
#include <iostream>
#include <memory>
#include <set>
#include <string>
#include <thread>
#include <tuple>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    if (i > 0) {
      std::cout << ", ";
    }
    std::cout << values[i];
  }
  std::cout << "]";
}

void print_robot_states(const std::shared_ptr<RobotStates>& robot_states) {
  if (!robot_states) {
    std::cout << "nullptr (use current complete robot state)" << std::endl;
    return;
  }

  std::cout << "RobotStates" << std::endl;
  std::cout << "    whole_body_joint[" << robot_states->whole_body_joint.size() << "]: ";
  print_vector(robot_states->whole_body_joint);
  std::cout << std::endl;
  std::cout << "    base_state[" << robot_states->base_state.size() << "]: ";
  print_vector(robot_states->base_state);
  std::cout << std::endl;
}

void print_get_jacobian_by_state_params(const std::string& chain_name,
                                        const std::string& target_frame,
                                        const std::string& reference_frame,
                                        const std::shared_ptr<RobotStates>& reference_robot_states) {
  std::cout << "get_jacobian_by_state parameters:" << std::endl;
  std::cout << "  chain_name: " << chain_name << std::endl;
  std::cout << "  target_frame: " << target_frame << std::endl;
  std::cout << "  reference_frame: " << reference_frame << std::endl;
  std::cout << "  reference_robot_states: ";
  print_robot_states(reference_robot_states);
}

void print_jacobian(const std::string& label,
                    const std::tuple<MotionStatus, std::vector<std::vector<double>>>& result,
                    GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& matrix = std::get<1>(result);

  std::cout << "[" << label << "] Status: " << motion.status_to_string(status) << std::endl;
  if (status != MotionStatus::SUCCESS || matrix.empty()) {
    std::cout << "Jacobian computation failed or returned an empty matrix." << std::endl;
    return;
  }

  std::cout << "Jacobian matrix (" << matrix.size() << "x" << matrix[0].size() << "):" << std::endl;
  for (size_t r = 0; r < matrix.size(); ++r) {
    std::cout << "  row " << r << ": ";
    for (double value : matrix[r]) {
      std::cout << std::setw(10) << std::fixed << std::setprecision(5) << value << " ";
    }
    std::cout << std::endl;
  }
}

std::vector<double> make_fixed_whole_body_joint(size_t dof) {
  static const std::vector<double> fixed_values = {
      0.3, 0.8, 0.5, 0.0, 0.0, 0.1, -0.2, 1.2, -0.8, 0.4, -1.0,
      0.2, -0.6, 0.0, -1.2, 0.8, -0.4, 1.0, -0.2, 0.6, 0.0};
  std::vector<double> values;
  values.reserve(dof);
  for (size_t i = 0; i < dof; ++i) {
    values.push_back(fixed_values[i % fixed_values.size()]);
  }
  return values;
}

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (!motion.init()) {
    std::cerr << "GalbotMotion initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotMotion initialized successfully" << std::endl;

  if (!robot.init()) {
    std::cerr << "GalbotRobot initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotRobot initialized successfully" << std::endl;

  std::this_thread::sleep_for(std::chrono::seconds(3));

  std::set<std::string> support_chains = motion.get_support_chains();
  std::cout << "support_chains (" << support_chains.size() << "): [";
  for (auto it = support_chains.begin(); it != support_chains.end(); ++it) {
    if (it != support_chains.begin()) {
      std::cout << ", ";
    }
    std::cout << *it;
  }
  std::cout << "]" << std::endl;
  if (support_chains.empty()) {
    support_chains = {"head", "left_arm", "right_arm", "torso", "leg"};
  }

  const std::string target_frame = "EndEffector";
  const std::string reference_frame = "base_link";

  size_t whole_body_dof = 21;
  try {
    RobotStates current_state = motion.get_robot_states();
    if (!current_state.whole_body_joint.empty()) {
      whole_body_dof = current_state.whole_body_joint.size();
    }
  } catch (const std::exception& e) {
    std::cerr << "get_robot_states exception: " << e.what() << std::endl;
  }

  auto reference_robot_states = std::make_shared<RobotStates>();
  reference_robot_states->whole_body_joint = make_fixed_whole_body_joint(whole_body_dof);
  reference_robot_states->base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

  // Compute the Jacobian for every chain using an explicit RobotStates
  for (const auto& chain_name : support_chains) {
    try {
      std::cout << "=== get_jacobian_by_state (explicit RobotStates): " << chain_name << " ===" << std::endl;
      print_get_jacobian_by_state_params(chain_name, target_frame, reference_frame, reference_robot_states);
      auto result = motion.get_jacobian_by_state(chain_name, target_frame, reference_frame, reference_robot_states);
      print_jacobian(chain_name + " explicit RobotStates", result, motion);
    } catch (const std::exception& e) {
      std::cerr << chain_name << " explicit RobotStates exception: " << e.what() << std::endl;
    }
  }

  // Compute the Jacobian for every chain using nullptr (the current robot state)
  for (const auto& chain_name : support_chains) {
    try {
      std::cout << "=== get_jacobian_by_state (nullptr RobotStates): " << chain_name << " ===" << std::endl;
      std::shared_ptr<RobotStates> null_robot_states = nullptr;
      print_get_jacobian_by_state_params(chain_name, target_frame, reference_frame, null_robot_states);
      auto result = motion.get_jacobian_by_state(chain_name, target_frame, reference_frame, null_robot_states);
      print_jacobian(chain_name + " nullptr RobotStates", result, motion);
    } catch (const std::exception& e) {
      std::cerr << chain_name << " nullptr RobotStates exception: " << e.what() << std::endl;
    }
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

通用逆运动学

适用场景：通过通用目标及约束配置求解复杂任务的逆运动学结果。

examples/cpp/galbot_motion/src/inverse_kinematics_general_example.cpp

#include <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

struct ExampleInputs {
  std::vector<double> left_arm_target_pose;
  MotionPlanWaypoint target_waypoint;
  std::shared_ptr<RobotStates> reference_state;
  std::shared_ptr<Parameter> params;
};

ExampleInputs make_example_inputs() {
  ExampleInputs inputs;

  // Fixed Cartesian pose for the left_arm IK target.
  // Pose layout: [x, y, z, qx, qy, qz, qw], reference frame: base_link.
  inputs.left_arm_target_pose = {
      0.405795,
      0.510630,
      0.872605,
      -0.0701184,
      -0.0226926,
      0.202097,
      0.976589,
  };

  MotionPlanChainTarget left_arm_target;
  left_arm_target.chain_name = "left_arm";
  left_arm_target.mode = MotionPlanTargetMode::kCartesian;
  left_arm_target.cart.chain_name = "left_arm";
  left_arm_target.cart.frame_id = "left_arm_end_effector_mount_link";
  left_arm_target.cart.reference_frame = "base_link";
  left_arm_target.cart.pose = Pose(inputs.left_arm_target_pose);
  left_arm_target.cart.assist_chains.insert("leg");

  // inverse_kinematics_general(target_waypoint, reference_state, params) input: target_waypoint.
  inputs.target_waypoint = {
      left_arm_target,
  };

  // inverse_kinematics_general input: reference_state. nullptr lets the service choose its default seed.
  inputs.reference_state = nullptr;

  // inverse_kinematics_general input: params.
  inputs.params = std::make_shared<Parameter>();
  inputs.params->set_direct_execute(false);
  inputs.params->set_blocking(true);
  inputs.params->set_timeout(20.0);
  inputs.params->set_check_collision(false);
  inputs.params->set_reference_frame("base_link");

  return inputs;
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize SDK interfaces directly in the example so users can see the required order.
  if (!motion.init()) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  const ExampleInputs inputs = make_example_inputs();
  std::cout << "Fixed target left_arm pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(inputs.left_arm_target_pose);
  std::cout << std::endl;

  // This is the API being demonstrated: solve joint values for the target waypoint.
  auto [status, joint_map] =
      motion.inverse_kinematics_general(inputs.target_waypoint, inputs.reference_state, inputs.params);

  std::cout << "inverse_kinematics_general status: " << motion.status_to_string(status) << std::endl;
  for (const auto& [chain, joints] : joint_map) {
    std::cout << "  " << chain << " joint names: ";
    for (const auto& name : joints.joint_names) {
      std::cout << name << " ";
    }
    std::cout << std::endl << "  " << chain << " joint positions: ";
    print_vector(joints.joint_positions);
    std::cout << std::endl;
  }

  // Clean shutdown releases SDK resources before the process exits.
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return status == MotionStatus::SUCCESS ? 0 : 1;
}

通用运动规划（motion_plan）

适用场景：为一个或多个机器人运动链构造并执行通用运动规划请求。

examples/cpp/galbot_motion/src/motion_plan_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
#include "galbot_sdk_type.hpp"

/*
 * This example focuses on the low-level multi-waypoint overload of
 * GalbotMotion::motion_plan():
 *
 *   motion_plan(const MotionPlanWaypoints& waypoints,
 *               const PlannerConfig& params,
 *               const std::shared_ptr<RobotStates>& start_state = nullptr)
 *
 * It demonstrates the main request-building features of that API:
 * - A MotionPlanWaypoints request is an ordered list of waypoints.
 * - Each waypoint can contain one target per chain, so the planner can
 *   coordinate both arms at the same waypoint index.
 * - MotionPlanChainTarget supports joint-space targets and Cartesian
 *   end-effector pose targets in the same request.
 * - Cartesian targets specify the controlled frame, the reference frame,
 *   and optional assist chains.
 * - Parameter / PlannerConfig carries common planning and execution settings
 *   such as direct execution, blocking behavior, timeout, collision checking,
 *   and reference frame.
 * - The return value is a status plus a trajectory map keyed by chain name.
 *
 * The example first moves the robot to the same fixed start posture used by the
 * test_newimpl examples, then sends the waypoint values from
 * motion_plan_targets.json.
 */
using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
  std::unordered_map<std::string, std::vector<double>> poses;
};

struct ExampleInputs {
  MotionPlanWaypoints waypoints;
  Parameter params;
};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

std::vector<double> pose_to_vector(const Pose& pose) {
  return {pose.position.x,    pose.position.y,    pose.position.z,   pose.orientation.x,
          pose.orientation.y, pose.orientation.z, pose.orientation.w};
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

bool move_robot_to_home_position(GalbotRobot& robot) {
  std::cout << "Move robot to the G1 home position." << std::endl;
  const std::vector<double> joint_positions = {
      0.5, 1.5, 1.0, 0.0, 0.0,
      0.0, 0.0,
      2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
      -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0};
  const std::vector<std::string> joint_group_names = {"leg", "head", "left_arm", "right_arm"};
  const ControlStatus status =
      robot.set_joint_positions(joint_positions, joint_group_names, {}, true, 0.1, 30.0);
  print_api_status("GalbotRobot::set_joint_positions(home)", status);
  return status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_joint_target(const std::string& chain, const std::vector<double>& joint_positions,
                                        const std::vector<std::string>& joint_names = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kJoint;
  target.joint.chain_name = chain;
  target.joint.joint_positions = joint_positions;
  target.joint.joint_names = joint_names;
  return target;
}

MotionPlanChainTarget make_cartesian_target(const std::string& chain, const std::vector<double>& pose,
                                            const std::vector<std::string>& assist_chains = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kCartesian;
  target.cart.chain_name = chain;
  target.cart.frame_id = "EndEffector";
  target.cart.reference_frame = "base_link";
  target.cart.pose = Pose(pose);
  target.cart.assist_chains.insert(assist_chains.begin(), assist_chains.end());
  return target;
}

bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

bool capture_current_pose(GalbotMotion& motion, GalbotRobot& robot, const std::string& chain,
                          CurrentRobotState& current) {
  if (!capture_current_joints(motion, robot, {chain}, current)) {
    return false;
  }

  auto [status, pose] = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link");
  print_api_status("get_end_effector_pose_on_chain(" + chain + ")", status, motion);
  if (status != MotionStatus::SUCCESS || pose.size() != 7) {
    std::cerr << "Failed to get current pose for chain: " << chain << std::endl;
    return false;
  }

  current.poses[chain] = pose;
  std::cout << "Current " << chain << " pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(pose);
  std::cout << std::endl;
  return true;
}

ExampleInputs make_example_inputs(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");

  inputs.waypoints = {
      {make_joint_target("left_arm", {1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_joint_target("left_arm", {1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_cartesian_target("left_arm", {0.32666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0}, {"torso"}),
       make_cartesian_target("right_arm", {0.32666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0}, {"torso"})},
      {make_cartesian_target("left_arm", {0.32666, 0.43435, 1.03569, 0.0, 0.0, 0.0, 1.0}, {"torso"}),
       make_cartesian_target("right_arm", {0.32666, -0.43435, 1.03569, 0.0, 0.0, 0.0, 1.0}, {"torso"})},
      {make_joint_target("left_arm", {1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names)}
      //  make_joint_target("leg", {0.499191, 1.49907, 1.00046, 0.0},
      //                    {"leg_joint1", "leg_joint2", "leg_joint3", "leg_joint4"})},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

void print_joint_names(const std::vector<std::string>& joint_names) {
  if (joint_names.empty()) {
    return;
  }
  std::cout << " joint_names=[";
  for (size_t i = 0; i < joint_names.size(); ++i) {
    std::cout << joint_names[i];
    if (i + 1 < joint_names.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

void print_waypoints(const MotionPlanWaypoints& waypoints) {
  std::cout << "motion_plan waypoints: " << waypoints.size() << std::endl;
  for (size_t i = 0; i < waypoints.size(); ++i) {
    std::cout << "  waypoint[" << i << "] targets: " << waypoints[i].size() << std::endl;
    for (const auto& target : waypoints[i]) {
      if (target.mode == MotionPlanTargetMode::kJoint) {
        std::cout << "    " << target.chain_name << " JOINT q=";
        print_vector(target.joint.joint_positions);
        print_joint_names(target.joint.joint_names);
        std::cout << std::endl;
      } else {
        std::cout << "    " << target.chain_name << " CART pose=";
        print_vector(pose_to_vector(target.cart.pose));
        std::cout << " frame_id=" << target.cart.frame_id
                  << " reference_frame=" << target.cart.reference_frame;
        if (!target.cart.assist_chains.empty()) {
          std::cout << " assist_chains=[";
          size_t assist_chain_index = 0;
          for (const auto& assist_chain : target.cart.assist_chains) {
            std::cout << assist_chain;
            if (++assist_chain_index < target.cart.assist_chains.size()) {
              std::cout << ", ";
            }
          }
          std::cout << "]";
        }
        std::cout << std::endl;
      }
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_home_position(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_pose(motion, robot, "left_arm", current) ||
      !capture_current_pose(motion, robot, "right_arm", current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs(current);
  print_waypoints(inputs.waypoints);

  std::cout << "Immediate execution is enabled; waypoints are loaded from motion_plan_targets.json reference values."
            << std::endl;

  auto result = motion.motion_plan(inputs.waypoints, inputs.params);
  print_traj_result("motion_plan", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

笛卡尔直线运动（move_line）

适用场景：在笛卡尔空间规划并执行末端执行器直线运动。

examples/cpp/galbot_motion/src/move_line_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
  std::unordered_map<std::string, std::vector<double>> poses;
};

struct ExampleInputs {
  MotionPlanWaypoints waypoints;
  Parameter params;
};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

std::vector<double> pose_to_vector(const Pose& pose) {
  return {pose.position.x,    pose.position.y,    pose.position.z,   pose.orientation.x,
          pose.orientation.y, pose.orientation.z, pose.orientation.w};
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

bool move_robot_to_home_position(GalbotRobot& robot) {
  std::cout << "Move robot to the G1 home position." << std::endl;
  const std::vector<double> joint_positions = {
      0.5, 1.5, 1.0, 0.0, 0.0,
      0.0, 0.0,
      2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
      -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0};
  const std::vector<std::string> joint_group_names = {"leg", "head", "left_arm", "right_arm"};
  const ControlStatus status =
      robot.set_joint_positions(joint_positions, joint_group_names, {}, true, 0.1, 30.0);
  print_api_status("GalbotRobot::set_joint_positions(home)", status);
  return status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_cartesian_target(const std::string& chain, const std::vector<double>& pose) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kCartesian;
  target.cart.chain_name = chain;
  target.cart.frame_id = "EndEffector";
  target.cart.reference_frame = "base_link";
  target.cart.pose = Pose(pose);
  target.cart.assist_chains.insert("torso");
  return target;
}

bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

bool capture_current_pose(GalbotMotion& motion, GalbotRobot& robot, const std::string& chain,
                          CurrentRobotState& current) {
  if (!capture_current_joints(motion, robot, {chain}, current)) {
    return false;
  }

  auto [status, pose] = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link");
  print_api_status("get_end_effector_pose_on_chain(" + chain + ")", status, motion);
  if (status != MotionStatus::SUCCESS || pose.size() != 7) {
    std::cerr << "Failed to get current pose for chain: " << chain << std::endl;
    return false;
  }
  current.poses[chain] = pose;
  std::cout << "Current " << chain << " pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(pose);
  std::cout << std::endl;
  return true;
}

ExampleInputs make_example_inputs() {
  ExampleInputs inputs;

  inputs.waypoints = {
      {make_cartesian_target("left_arm", {0.42666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0}),
       make_cartesian_target("right_arm", {0.422666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0})},
      {make_cartesian_target("left_arm", {0.42666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0}),
       make_cartesian_target("right_arm", {0.422666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0})},
      {make_cartesian_target("left_arm", {0.12666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0}),
       make_cartesian_target("right_arm", {0.12666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0})},
      {make_cartesian_target("left_arm", {0.12666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0}),
       make_cartesian_target("right_arm", {0.12666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0})},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

void print_waypoints(const MotionPlanWaypoints& waypoints) {
  std::cout << "move_line waypoints: " << waypoints.size() << std::endl;
  for (size_t i = 0; i < waypoints.size(); ++i) {
    std::cout << "  waypoint[" << i << "] targets: " << waypoints[i].size() << std::endl;
    for (const auto& target : waypoints[i]) {
      std::cout << "    " << target.chain_name << " CART pose=";
      print_vector(pose_to_vector(target.cart.pose));
      std::cout << " frame_id=" << target.cart.frame_id
                << " reference_frame=" << target.cart.reference_frame;
      if (!target.cart.assist_chains.empty()) {
        std::cout << " assist_chains=[";
        size_t assist_chain_index = 0;
        for (const auto& assist_chain : target.cart.assist_chains) {
          std::cout << assist_chain;
          if (++assist_chain_index < target.cart.assist_chains.size()) {
            std::cout << ", ";
          }
        }
        std::cout << "]";
      }
      std::cout << std::endl;
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_home_position(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_pose(motion, robot, "left_arm", current) ||
      !capture_current_pose(motion, robot, "right_arm", current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs();
  print_waypoints(inputs.waypoints);

  std::cout << "Immediate execution is enabled; waypoints are loaded from move_line_targets.json reference values."
            << std::endl;

  auto result = motion.move_line(inputs.waypoints, inputs.params);
  print_traj_result("move_line", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

雅可比接口测试

适用场景：验证不同运动链、目标坐标系和指定机器人状态下的雅可比矩阵计算。

examples/cpp/galbot_motion/src/test_get_jacobian.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

static int g_test_count = 0;
static int g_pass_count = 0;
static int g_fail_count = 0;

void print_jacobian(const std::string& label,
                    const std::tuple<MotionStatus, std::vector<std::vector<double>>>& res,
                    GalbotMotion& planner) {
    auto status = std::get<0>(res);
    const auto& matrix = std::get<1>(res);

    std::cout << "[" << label << "] Status: " << planner.status_to_string(status) << std::endl;

    if (status == MotionStatus::SUCCESS && !matrix.empty()) {
        int rows = static_cast<int>(matrix.size());
        int cols = static_cast<int>(matrix[0].size());
        std::cout << "Jacobian matrix (" << rows << "x" << cols << "):" << std::endl;
        for (int r = 0; r < rows; ++r) {
            std::cout << "  row " << r << ": ";
            for (int c = 0; c < cols; ++c) {
                printf("%10.5f ", matrix[r][c]);
            }
            std::cout << std::endl;
        }
    }
    std::cout << std::endl;
}

void run_test(const std::string& name, MotionStatus expected,
              const std::tuple<MotionStatus, std::vector<std::vector<double>>>& res,
              GalbotMotion& planner) {
    g_test_count++;
    auto status = std::get<0>(res);
    const auto& matrix = std::get<1>(res);

    bool ok = (status == expected);
    bool shape_ok = true;
    if (ok && status == MotionStatus::SUCCESS) {
        shape_ok = (!matrix.empty() && matrix[0].size() > 0);
        // Jacobian should be 6 rows (vx,vy,vz,wx,wy,wz)
        if (shape_ok) {
            shape_ok = (matrix.size() == 6);
        }
    }

    if (ok && shape_ok) {
        g_pass_count++;
        std::cout << "[PASS] " << name << std::endl;
    } else {
        g_fail_count++;
        std::cout << "[FAIL] " << name;
        if (!ok) {
            std::cout << " (expected: " << planner.status_to_string(expected)
                      << ", got: " << planner.status_to_string(status) << ")";
        }
        if (!shape_ok && status == MotionStatus::SUCCESS) {
            std::cout << " (unexpected matrix shape)";
        }
        std::cout << std::endl;
    }
    print_jacobian(name, res, planner);
}

int main() {
    auto& planner = GalbotMotion::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!planner.init()) {
        std::cerr << "Planner initialization failed!" << std::endl;
        return -1;
    }
    std::cout << "Planner initialized successfully!" << std::endl;

    if (!robot.init()) {
        std::cerr << "Robot initialization failed!" << std::endl;
        return -1;
    }
    std::cout << "Robot initialized successfully!" << std::endl;

    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    // --- Print supported links (valid target_frame link names for get_jacobian) ---
    std::cout << "\n=== Supported links (valid target_frame for get_jacobian) ===" << std::endl;
    {
        const auto& support_links = planner.get_support_links();
        std::cout << "get_support_links() returned " << support_links.size() << " links:" << std::endl;
        for (const auto& link : support_links) {
            std::cout << "  - " << link << std::endl;
        }

        const auto ee_links = planner.get_link_names(true);
        std::cout << "get_link_names(only_end_effector=true) returned " << ee_links.size()
                  << " end-effector links:" << std::endl;
        for (const auto& link : ee_links) {
            std::cout << "  - " << link << std::endl;
        }
        std::cout << std::endl;
    }

    // Joint configurations for testing
    std::vector<double> left_arm_joints = {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000};
    std::vector<double> right_arm_joints = {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000};

    // --- Test 1: Default parameters (current robot state) ---
    try {
        std::cout << "=== Test 1: get_jacobian with default parameters ===" << std::endl;
        auto res = planner.get_jacobian("left_arm");
        run_test("left_arm default state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 1 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 2: EndEffector frame, base_link reference ---
    try {
        std::cout << "=== Test 2: EndEffector frame / base_link reference ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "EndEffector", "base_link");
        run_test("left_arm EndEffector/base_link", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 2 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 3: Tool frame, world reference (NOT supported -> INVALID_INPUT) ---
    try {
        std::cout << "=== Test 3: Tool frame / world reference (expected INVALID_INPUT) ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "Tool", "world");
        run_test("left_arm Tool/world", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 3 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 4: Custom joint state ---
    try {
        std::cout << "=== Test 4: Custom joint state ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> custom_joint_state = {{"left_arm", left_arm_joints}};
        auto custom_param_ptr = std::make_shared<Parameter>();
        custom_param_ptr->set_timeout(5.0);
        auto res = planner.get_jacobian("left_arm", "EndEffector", "base_link", custom_joint_state, custom_param_ptr);
        run_test("left_arm custom joint state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 4 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 5: Right arm, default state ---
    try {
        std::cout << "=== Test 5: Right arm default state ===" << std::endl;
        auto res = planner.get_jacobian("right_arm");
        run_test("right_arm default state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 5 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 6: Right arm with custom joint state ---
    try {
        std::cout << "=== Test 6: Right arm custom joint state ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> custom_joint_state = {{"right_arm", right_arm_joints}};
        auto res = planner.get_jacobian("right_arm", "EndEffector", "base_link", custom_joint_state);
        run_test("right_arm custom joint state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 6 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 7: Invalid chain name ---
    try {
        std::cout << "=== Test 7: Invalid chain name ===" << std::endl;
        auto res = planner.get_jacobian("invalid_chain");
        run_test("invalid_chain", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 7 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 8: Left arm, EndEffector frame, world reference ---
    try {
        std::cout << "=== Test 8: EndEffector / world reference ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "EndEffector", "world");
        run_test("left_arm EndEffector/world", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 8 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 9: Left arm, Tool frame, base_link reference (NOT supported -> INVALID_INPUT) ---
    try {
        std::cout << "=== Test 9: Tool frame / base_link reference (expected INVALID_INPUT) ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "Tool", "base_link");
        run_test("left_arm Tool/base_link", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 9 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 10: Both arms sequentially ---
    try {
        std::cout << "=== Test 10: Both arms ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> both_arms_joint;
        both_arms_joint["left_arm"] = left_arm_joints;
        both_arms_joint["right_arm"] = right_arm_joints;

        auto res_left = planner.get_jacobian("left_arm", "EndEffector", "base_link", both_arms_joint);
        run_test("both_arms left", MotionStatus::SUCCESS, res_left, planner);

        auto res_right = planner.get_jacobian("right_arm", "EndEffector", "base_link", both_arms_joint);
        run_test("both_arms right", MotionStatus::SUCCESS, res_right, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 10 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 11: Empty chain name ---
    try {
        std::cout << "=== Test 11: Empty chain name ===" << std::endl;
        auto res = planner.get_jacobian("");
        run_test("empty_chain", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 11 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 12: All zeros joint configuration ---
    try {
        std::cout << "=== Test 12: All zeros joint configuration ===" << std::endl;
        std::vector<double> zero_joints(7, 0.0);
        std::unordered_map<std::string, std::vector<double>> zero_joint_state = {{"left_arm", zero_joints}};
        auto res = planner.get_jacobian("left_arm", "EndEffector", "base_link", zero_joint_state);
        run_test("left_arm zero joints", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 12 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 13: torso chain (torso shares the leg end-effector: leg_end_effector_mount_link) ---
    try {
        std::cout << "=== Test 13: torso chain / EndEffector / base_link ===" << std::endl;
        auto res = planner.get_jacobian("torso", "EndEffector", "base_link");
        run_test("torso EndEffector/base_link", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 13 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 14: leg chain with joint values (EndEffector maps to leg_end_effector_mount_link;
    //     G1 leg DOF = 5, so pass five joint values) ---
    try {
        std::cout << "=== Test 14: leg chain / EndEffector / base_link (5 joints) ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> leg_joint_state = {
            {"leg", {0.0, 0.0, 0.0, 0.0, 0.0}}};
        auto res = planner.get_jacobian("leg", "EndEffector", "base_link", leg_joint_state);
        run_test("leg EndEffector/base_link", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 14 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // Summary
    std::cout << "\n========================================" << std::endl;
    std::cout << "Jacobian test summary: " << g_test_count << " tests, "
              << g_pass_count << " passed, " << g_fail_count << " failed" << std::endl;
    std::cout << "========================================\n" << std::endl;

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return (g_fail_count == 0) ? 0 : 1;
}

轨迹规划（traj_plan）

适用场景：为已构造的运动规划请求生成带时间参数的轨迹。

examples/cpp/galbot_motion/src/traj_plan_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
};

struct ExampleInputs {
  MotionPlanWaypoints waypoints;
  Parameter params;
};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

bool move_robot_to_home_position(GalbotRobot& robot) {
  std::cout << "Move robot to the G1 home position." << std::endl;
  const std::vector<double> joint_positions = {
      0.5, 1.5, 1.0, 0.0, 0.0,
      0.0, 0.0,
      2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
      -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0};
  const std::vector<std::string> joint_group_names = {"leg", "head", "left_arm", "right_arm"};
  const ControlStatus status =
      robot.set_joint_positions(joint_positions, joint_group_names, {}, true, 0.1, 30.0);
  print_api_status("GalbotRobot::set_joint_positions(home)", status);
  return status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_joint_target(const std::string& chain, const std::vector<double>& joint_positions,
                                        const std::vector<std::string>& joint_names = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kJoint;
  target.joint.chain_name = chain;
  target.joint.joint_positions = joint_positions;
  target.joint.joint_names = joint_names;
  return target;
}

bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

ExampleInputs make_example_inputs(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");

  inputs.waypoints = {
      {make_joint_target("left_arm", {1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names)},
      {make_joint_target("left_arm", {1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names)},
      {make_joint_target("left_arm", {1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0},
                         right_arm_joint_names)},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_chain_only");

  return inputs;
}

ExampleInputs make_example_inputs_with_leg(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");
  const std::vector<std::string> leg_joint_names = current.joint_names.at("leg");
  std::vector<double> leg_target = current.joints.at("leg");
  leg_target[3] = 0.0;
  leg_target[4] = 0.0;

  inputs.waypoints = {
      {make_joint_target("left_arm", {1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names),
       make_joint_target("leg", leg_target, leg_joint_names)},
      {make_joint_target("left_arm", {1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names),
       make_joint_target("leg", leg_target, leg_joint_names)},
      {make_joint_target("left_arm", {1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0},
                         right_arm_joint_names),
       make_joint_target("leg", leg_target, leg_joint_names)},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_leg");

  return inputs;
}

ExampleInputs make_example_inputs_with_torso(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");

  inputs.waypoints = {
      {make_joint_target("left_arm", {1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_joint_target("left_arm", {1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_joint_target("left_arm", {1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

void print_waypoints(const MotionPlanWaypoints& waypoints) {
  std::cout << "traj_plan waypoints: " << waypoints.size() << std::endl;
  for (size_t i = 0; i < waypoints.size(); ++i) {
    std::cout << "  waypoint[" << i << "] targets: " << waypoints[i].size() << std::endl;
    for (const auto& target : waypoints[i]) {
      if (target.mode == MotionPlanTargetMode::kJoint) {
        std::cout << "    " << target.chain_name << " JOINT q=";
        print_vector(target.joint.joint_positions);
        if (!target.joint.joint_names.empty()) {
          std::cout << " joint_names=[";
          for (size_t j = 0; j < target.joint.joint_names.size(); ++j) {
            std::cout << target.joint.joint_names[j];
            if (j + 1 < target.joint.joint_names.size()) {
              std::cout << ", ";
            }
          }
          std::cout << "]";
        }
        std::cout << std::endl;
      } else {
        std::cout << "    " << target.chain_name << " unexpected CART target for traj_plan" << std::endl;
      }
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_home_position(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_joints(motion, robot, {"leg", "left_arm", "right_arm"}, current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs_with_torso(current);
  // const ExampleInputs inputs = make_example_inputs(current);
  print_waypoints(inputs.waypoints);

  std::cout << "Immediate execution is enabled; waypoints are loaded from motion_plan_targets.json reference values."
            << std::endl;

  auto result = motion.traj_plan(inputs.waypoints, inputs.params);
  print_traj_result("traj_plan", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

类：GalbotNavigation

获取实例 / 初始化(get_instance_init)

适用场景：获取导航模块单例并初始化。必须在使用导航功能前调用。

examples/cpp/galbot_navigation/src/get_instance_init_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    std::cout << "Initialization successful!" << std::endl;

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

重定位 / 是否已定位 / 获取当前位姿(relocation)

适用场景：触发重定位，检查机器人是否已经成功定位，并获取机器人在地图中的当前位姿。

examples/cpp/galbot_navigation/src/relocation_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << "relocalization success." << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                  << current_pose.position.y << ", " << current_pose.position.z
                  << "), orientation(" << current_pose.orientation.x << ", "
                  << current_pose.orientation.y << ", " << current_pose.orientation.z
                  << ", " << current_pose.orientation.w << ")" << std::endl;

        robot.request_shutdown();
        robot.wait_for_shutdown();
    } else {
        std::cout << "relocalization failed, cannot proceed with navigation." << std::endl;
    }

    robot.destroy();

    return 0;
}

检查路径可达性并阻塞导航到目标(blocked_navigation)

适用场景：检查目标点是否可达，如果可达则阻塞等待导航完成到达目标。简单场景使用方便。

examples/cpp/galbot_navigation/src/blocked_navigation_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << "relocalization success." << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        std::this_thread::sleep_for(std::chrono::milliseconds(2000));

        // Whether to enable obstacle checking (can be set to true in open environments)
        bool enable_collision_check = false;
        // Whether to block and wait for arrival
        bool is_blocking = true;
        // Maximum wait time to reach position
        float timeout_s = 20;

        // Navigate 3 times in loop
        int count = 0;
        while (count++ < 3) {
            std::cout << "No. " << count << " navigation(s)" << std::endl;
            // checkpath navigation target
            if (navigation.check_path_reachability(goal_pose, init_pose)) {
                std::cout << "Path reachable, navigating to target point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    goal_pose, enable_collision_check, is_blocking, timeout_s);
                if (status == NavigationStatus::SUCCESS) {
                    std::cout << "Target point reached" << std::endl;
                } else {
                    std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
            }
            // Check path reachability and return to start point
            if (navigation.check_path_reachability(init_pose, goal_pose)) {
                std::cout << "Path reachable, navigating to start point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    init_pose, enable_collision_check, is_blocking, timeout_s);
                if (status == NavigationStatus::SUCCESS) {
                    std::cout << "Returned to start point" << std::endl;
                } else {
                    std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to start point" << std::endl;
            }
        }

        // Stop navigation
        navigation.stop_navigation();

        // Get current pose again
        current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

    } else {
        std::cout << "relocalization failed." << std::endl;
    }


    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

非阻塞导航 + 轮询判断到达(non_blocking_navigation)

适用场景：启动导航但不阻塞当前线程，可以在导航过程中做其他处理，需要轮询判断是否到达目标。适合需要异步处理的场景。

examples/cpp/galbot_navigation/src/non_blocking_navigation_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }
    if (navigation.is_localized()) {
        std::cout << "relocalization success." << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        std::this_thread::sleep_for(std::chrono::milliseconds(2000));

        // Whether to enable obstacle checking (can be set to true in open environments)
        bool enable_collision_check = false;
        // Whether to block and wait for arrival
        bool is_blocking = false;

        // Navigate 2 times in loop, non-blocking wait for arrival
        int count = 0;
        while (count++ < 2) {
            std::cout << "No. " << count << " navigation(s)" << std::endl;
            // checkpath navigation target
            if (navigation.check_path_reachability(goal_pose, init_pose)) {
                std::cout << "Path reachable, navigating to target point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    goal_pose, enable_collision_check, is_blocking);
                // wait
                int count_arrival = 0;
                while (!navigation.check_goal_arrival()) {
                    std::cout << "navigate has not arrived" << std::endl;
                    std::this_thread::sleep_for(std::chrono::milliseconds(500));
                    if (++count_arrival > 10) {
                        break;
                    }
                }
                if (navigation.check_goal_arrival()) {
                    std::cout << "Target point reached" << std::endl;
                } else {
                    std::cout << "Navigation failed; target point not reached" << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
            }
            // Check path reachability and return to start point
            if (navigation.check_path_reachability(init_pose, goal_pose)) {
                std::cout << "Path reachable, navigating to start point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    init_pose, enable_collision_check, is_blocking);
                // wait
                int count_arrival = 0;
                while (!navigation.check_goal_arrival()) {
                    std::cout << "navigate has not arrived" << std::endl;
                    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                    if (++count_arrival > 10) {
                        break;
                    }
                }
                if (navigation.check_goal_arrival()) {
                    std::cout << "Target point reached" << std::endl;
                } else {
                    std::cout << "Navigation failed; target point not reached" << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to start point" << std::endl;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }

        // Stop navigation
        navigation.stop_navigation();

        // Get current pose
        current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;
    } else {
        std::cout << "relocalization failed, cannot proceed with navigation." << std::endl;
    }
    
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

获取导航状态 + 轮询 SUCCESS/FAILED 或超时(get_navigation_status_example)

适用场景：获取当前导航任务的执行状态，用于非阻塞导航时轮询。

examples/cpp/galbot_navigation/src/get_navigation_status_example.cpp

/**
 * example: navigation get_navigation_status, SUCCESS/FAILED timeout exit,
 * Avoid deadlock and execute error logic.
 */
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose goal_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    const double timeout_s = 20.0;
    const double poll_interval_s = 0.5;

    // Non-blocking navigation
    navigation.navigate_to_goal(goal_pose, true, false, static_cast<float>(timeout_s));
    auto start = std::chrono::steady_clock::now();

    while (true) {
        NavigationTaskStatus status = navigation.get_navigation_status();
        auto elapsed = std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count();

        if (status == NavigationTaskStatus::SUCCESS) {
            std::cout << "Target reached" << std::endl;
            break;
        }
        if (status == NavigationTaskStatus::FAILED) {
            std::cout << "Navigation failed; exit error-handling logic promptly" << std::endl;
            break;
        }
        if (elapsed >= timeout_s) {
            std::cout << "navigationtimeout, exit" << std::endl;
            break;
        }

        if (status == NavigationTaskStatus::RUNNING) {
            std::cout << "Navigating... Status: RUNNING, elapsed: " << elapsed << "s" << std::endl;
        } else {
            std::cout << "Status: UNKNOWN, : " << elapsed << "s" << std::endl;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(poll_interval_s * 1000)));
    }

    // Stop navigation
    navigation.stop_navigation();

    // Get current pose again
    Pose current_pose = navigation.get_current_pose();
    std::cout << "Current pose: Position(" << current_pose.position.x << ", "
            << current_pose.position.y << ", " << current_pose.position.z
            << "), orientation(" << current_pose.orientation.x << ", "
            << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << "Resources released successfully" << std::endl;
    return 0;
}

直线移动到目标 / 停止导航(linear_movement)

适用场景：让机器人沿直线移动到目标点，或停止当前正在进行的导航任务。

examples/cpp/galbot_navigation/src/linear_movement_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <chrono>
#include <iomanip>
#include <sstream>

using namespace galbot::sdk;

// Helper function: get current time string [HH:MM:SS.ms]
std::string get_timestamp() {
    auto now = std::chrono::system_clock::now();
    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
    std::time_t t = std::chrono::system_clock::to_time_t(now);
    std::tm tm = *std::localtime(&t);

    std::stringstream ss;
    ss << "[" << std::put_time(&tm, "%H:%M:%S") << "." 
       << std::setfill('0') << std::setw(3) << ms.count() << "] ";
    return ss.str();
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    std::cout << get_timestamp() << "Starting call to robot.init()..." << std::endl;
    if (robot.init()) {
        std::cout << get_timestamp() << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << get_timestamp() << "Base instance initialization failed!" << std::endl;
        return -1;
    }

    std::cout << get_timestamp() << "Starting call to navigation.init()..." << std::endl;
    if (navigation.init()) {
        std::cout << get_timestamp() << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << get_timestamp() << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    std::cout << get_timestamp() << "Enter relocalization check loop..." << std::endl;
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        std::cout << get_timestamp() << "Calling navigation.relocalize()..." << std::endl;
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << get_timestamp() << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << get_timestamp() << "relocalization success." << std::endl;

        // Get current pose
        std::cout << get_timestamp() << "Calling navigation.get_current_pose()..." << std::endl;
        Pose current_pose = navigation.get_current_pose();
        std::cout << get_timestamp() << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        // Whether to block and wait for arrival
        bool is_blocking = true;
        // Maximum wait time to reach position
        float timeout_s = 20;

        // --- ---
        std::cout << "--------------------------------------------------" << std::endl;
        std::cout << get_timestamp() << "Preparing to execute move_straight_to (linear movement)..." << std::endl;
        
        // Record start time
        auto start_move = std::chrono::system_clock::now();
        
        // Execute move
        NavigationStatus status = navigation.move_straight_to(goal_pose, is_blocking, timeout_s);
        
        // Record end time and calculate elapsed time
        auto end_move = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_move - start_move).count();

        if (status == NavigationStatus::SUCCESS) {
            std::cout << get_timestamp() << "Target point reached (elapsed: " << duration << "ms)" << std::endl;
        } else {
            std::cout << get_timestamp() << "navigationfailed, status: " << static_cast<int>(status) 
                    << " (elapsed: " << duration << "ms)" << std::endl;
        }
        std::cout << "--------------------------------------------------" << std::endl;

        // Stop navigation
        std::cout << get_timestamp() << "Calling navigation.stop_navigation()..." << std::endl;
        navigation.stop_navigation();

        // Get current pose again
        std::cout << get_timestamp() << "Calling navigation.get_current_pose()..." << std::endl;
        current_pose = navigation.get_current_pose();
        std::cout << get_timestamp() << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;
    } else {
        std::cout << get_timestamp() << "Relocalization failed, cannot continue navigation." << std::endl;
    }
    
    std::cout << get_timestamp() << "Executing shutdown..." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << get_timestamp() << "Program finished." << std::endl;

    return 0;
}

完整运行示例（简单流程）(complete_running_example)

适用场景：展示导航功能的完整使用流程，包括初始化、重定位、导航到目标等步骤，供参考。

examples/cpp/galbot_navigation/src/complete_running_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << "Relocalization successful!" << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        // Whether to enable obstacle checking (can be set to true in open environments)
        bool enable_collision_check = false;
        // Whether to block and wait for arrival
        bool is_blocking = true;
        // Maximum wait time to reach position
        float timeout_s = 20;

        // checkpath navigation target
        if (navigation.check_path_reachability(goal_pose, init_pose)) {
            std::cout << "Path reachable, navigating to target point" << std::endl;
            // navigation, obstaclecheck, wait, wait 20
            NavigationStatus status = navigation.navigate_to_goal(
                goal_pose, enable_collision_check, is_blocking, timeout_s);
            if (status == NavigationStatus::SUCCESS) {
                std::cout << "Target point reached" << std::endl;
            } else {
                std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
            }
        } else {
            std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
        }

        // Check path reachability and return to start point
        if (navigation.check_path_reachability(init_pose, goal_pose)) {
            std::cout << "Path reachable, navigating to start point" << std::endl;
            // Navigate to the target waypoint with obstacle checking disabled and non-blocking wait for arrival
            is_blocking = false;
            NavigationStatus status = navigation.navigate_to_goal(
                init_pose, enable_collision_check, is_blocking);
            // wait
            int count_arrival = 0;
            while (!navigation.check_goal_arrival()) {
                std::cout << "navigate has not arrived" << std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                if (++count_arrival > 10) {
                    break;
                }
            }
            if (navigation.check_goal_arrival()) {
                std::cout << "Target point reached" << std::endl;
            } else {
                std::cout << "Navigation failed; target point not reached" << std::endl;
            }
        } else {
            std::cout << "Path unreachable, cannot navigate to start point" << std::endl;
        }

        // checkpath navigation target
        if (navigation.check_path_reachability(goal_pose, init_pose)) {
            std::cout << "Path reachable, navigating to target point" << std::endl;
            // target, wait, wait 10
            is_blocking = true;
            NavigationStatus status = navigation.move_straight_to(
                goal_pose, is_blocking, timeout_s);

            if (status == NavigationStatus::SUCCESS) {
                std::cout << "Target point reached" << std::endl;
            } else {
                std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
            }
        } else {
            std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
        }

        // Stop navigation
        navigation.stop_navigation();

    } else {
        std::cout << "Relocalization failed!" << std::endl;
    }


    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

携带物体时的导航包围盒过滤

适用场景：机器人携带附着物体导航时，过滤对应的导航碰撞几何体。

examples/cpp/galbot_navigation/src/bounding_box_filter_example.cpp

/**
 * example: add/get/remove navigation bounding boxes and attach/detach box collision objects.
 *
 * These boxes are used by navigation fusion to ignore corresponding box regions,
 * so navigation will not treat the carried/known boxes as obstacles.
 */
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <vector>

using namespace galbot::sdk;

void print_box_info(const BoxInfo& box_info) {
    std::cout << "box_tag: " << box_info.box_tag << std::endl;
    std::cout << "parent_link_name: " << box_info.parent_link_name << std::endl;
    std::cout << "box_size: [" << box_info.box_size.length_x << ", "
              << box_info.box_size.length_y << ", " << box_info.box_size.length_z << "]" << std::endl;
    std::cout << "box_pose: [" << box_info.box_pose.position.x << ", "
              << box_info.box_pose.position.y << ", " << box_info.box_pose.position.z << ", "
              << box_info.box_pose.orientation.x << ", " << box_info.box_pose.orientation.y << ", "
              << box_info.box_pose.orientation.z << ", " << box_info.box_pose.orientation.w << "]" << std::endl;
}

BoxInfo make_box_info(int box_tag, const BoxSize& box_size, const Pose& box_pose,
                      const std::string& parent_link_name) {
    BoxInfo box_info;
    box_info.box_tag = box_tag;
    box_info.box_size = box_size;
    box_info.box_pose = box_pose;
    box_info.parent_link_name = parent_link_name;
    return box_info;
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    BoxSize small_box_size{0.4f, 0.3f, 0.28f};
    Pose small_box_pose(std::vector<double>{0.45, 0.0, 0.25, 0.0, 0.0, 0.0, 1.0});
    Pose attached_box_pose(std::vector<double>{0.2, 0.0, -0.2, 0.0, 0.0, 0.0, 1.0});
    BoxInfo small_box = make_box_info(1001, small_box_size, small_box_pose, "base_link");
    BoxInfo attached_box = make_box_info(2001, small_box_size, attached_box_pose, "left_arm_end_effector_mount_link");

    std::cout << "Add navigation bounding box filters" << std::endl;
    auto add_status = navigation.add_bounding_box(small_box);
    std::cout << "add_bounding_box result: " << static_cast<int>(add_status) << std::endl;

    std::cout << "Get navigation bounding box filters" << std::endl;
    auto boxes = navigation.get_bounding_box();
    std::cout << "box count: " << boxes.size() << std::endl;
    for (const auto& box : boxes) {
        print_box_info(box);
    }

    std::cout << "Attach one box collision object to a robot link" << std::endl;
    std::vector<std::string> ignore_collision_links;
    auto attach_status = navigation.attach_box_to_link(attached_box, ignore_collision_links);
    std::cout << "attach_box_to_link result: " << static_cast<int>(attach_status) << std::endl;

    std::cout << "Detach one box collision object from its robot link" << std::endl;
    auto detach_status = navigation.detach_box_from_link(2001);
    std::cout << "detach_box_from_link result: " << static_cast<int>(detach_status) << std::endl;

    std::cout << "Remove one navigation bounding box filter" << std::endl;
    auto remove_status = navigation.remove_bounding_box(1001);
    std::cout << "remove_bounding_box result: " << static_cast<int>(remove_status) << std::endl;

    boxes = navigation.get_bounding_box();
    std::cout << "box count after remove: " << boxes.size() << std::endl;
    for (const auto& box : boxes) {
        print_box_info(box);
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << "Resources released successfully" << std::endl;
    return 0;
}

沿轨迹导航（navigate_along_trajectory）

适用场景：执行由一组有序底盘位姿构成的导航轨迹。

examples/cpp/galbot_navigation/src/navigate_along_trajectory_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <chrono>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;
constexpr float kTaskTimeoutSec = 100.0f;

std::string navigation_target_status_to_string(NavigationTaskStatus status) {
    switch (status) {
        case NavigationTaskStatus::UNKNOWN:
            return "UNKNOWN";
        case NavigationTaskStatus::RUNNING:
            return "RUNNING";
        case NavigationTaskStatus::SUCCESS:
            return "SUCCESS";
        case NavigationTaskStatus::FAILED:
            return "FAILED";
        case NavigationTaskStatus::INTERRUPTED:
            return "INTERRUPTED";
        case NavigationTaskStatus::OCCUPIED:
            return "OCCUPIED";
        case NavigationTaskStatus::COLLISION:
            return "COLLISION";
        case NavigationTaskStatus::CLOSE_TO_OBSTACLE:
            return "CLOSE_TO_OBSTACLE";
        default:
            return "UNKNOWN";
    }
}

bool is_terminal_status(NavigationTaskStatus status) {
    return status == NavigationTaskStatus::SUCCESS || status == NavigationTaskStatus::FAILED ||
           status == NavigationTaskStatus::INTERRUPTED || status == NavigationTaskStatus::OCCUPIED ||
           status == NavigationTaskStatus::COLLISION || status == NavigationTaskStatus::CLOSE_TO_OBSTACLE;
}

void print_task_snapshot(const TaskHandle& handle, const NavigationTaskSnapshot& snapshot) {
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
    Pose current_pose = navigation.get_current_pose();
    std::cout << indent << "current_pose: ["
              << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
              << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
              << current_pose.orientation.w << "]" << std::endl;
}

void monitor_task_until_terminal(GalbotNavigation& navigation, const TaskHandle& handle, float timeout_s) {
    const auto start = std::chrono::steady_clock::now();
    while (true) {
        NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
        print_task_snapshot(handle, snapshot);
        print_current_pose(navigation, "    ");

        const auto now = std::chrono::steady_clock::now();
        const double elapsed = std::chrono::duration<double>(now - start).count();

        if (is_terminal_status(snapshot.status)) {
            return;
        }

        if (elapsed >= timeout_s) {
            std::cout << "  timeout reached while waiting for task completion." << std::endl;
            return;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
}

}  // namespace

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
    int retry = 0;
    while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
        ++retry;
    }
    return navigation.is_localized();
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    /* All coordinates are in the "map" frame. */
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    std::vector<Pose> waypoints = {
        {std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}},
        {std::vector<double>{0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}},
        {std::vector<double>{1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}}
    };

    /* Wait for data preparation */
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    if (!wait_for_localization(navigation, init_pose)) {
        std::cerr << "localization failed" << std::endl;
        robot.request_shutdown();
        robot.wait_for_shutdown();
        robot.destroy();
        return -1;
    }

    // Print current pose
    print_current_pose(navigation);

    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    std::cout << "set target: waypoints" << std::endl;
    for (size_t i = 0; i < waypoints.size(); ++i) {
        std::cout << "  waypoint_" << i << ": ["
                  << waypoints[i].position.x << ", " << waypoints[i].position.y << ", " << waypoints[i].position.z << ", "
                  << waypoints[i].orientation.x << ", " << waypoints[i].orientation.y << ", " << waypoints[i].orientation.z << ", "
                  << waypoints[i].orientation.w << "]" << std::endl;
    }

    // Whether to enable obstacle checking (can be set to true in open environments)
    bool enable_collision_check = false;
    // The reference frame of the trajectory (can be set to "map" or "base_link", Defaulting to "map")
    const std::string frame_id = "map";
    // Speed scaling ratio for this trajectory execution, 1.0 means full speed.
    float speed_ratio = 1.0f;
    constexpr float timeout_s = kTaskTimeoutSec;

    TaskHandle handle =
        navigation.navigate_along_trajectory(waypoints, frame_id, speed_ratio, enable_collision_check);

    if (handle.request_sent) {
        monitor_task_until_terminal(navigation, handle, timeout_s);
    } else {
        std::cout << "trajectory task submission failed" << std::endl;
    }

    std::cout << "\nfinal task summary" << std::endl;
    NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;

    // Stop navigation
    navigation.stop_navigation();

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

导航至目标 V2（navigate_to_goal_v2）

适用场景：使用扩展目标接口及其附加选项导航至指定目标位姿。

examples/cpp/galbot_navigation/src/navigate_to_goal_v2_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <array>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;

std::string navigation_status_to_string(NavigationStatus status) {
  switch (status) {
    case NavigationStatus::SUCCESS:
      return "SUCCESS";
    case NavigationStatus::FAIL:
      return "FAIL";
    case NavigationStatus::TIMEOUT:
      return "TIMEOUT";
    case NavigationStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case NavigationStatus::MODE_ERR:
      return "MODE_ERR";
    case NavigationStatus::COMM_ERR:
      return "COMM_ERR";
    case NavigationStatus::WAIT_INITIALIZED:
      return "WAIT_INITIALIZED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
  Pose current_pose = navigation.get_current_pose();
  std::cout << indent << "current_pose: ["
            << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
            << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
            << current_pose.orientation.w << "]" << std::endl;
}

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
  int retry = 0;
  while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
    navigation.relocalize(init_pose);
    std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
    ++retry;
  }
  return navigation.is_localized();
}

}  // namespace

int main() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  auto controller_status = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

  std::this_thread::sleep_for(std::chrono::milliseconds(3000));

  if (!wait_for_localization(navigation, init_pose)) {
    std::cerr << "localization failed" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  print_current_pose(navigation);

  const Pose relative_goal(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
  const std::array<double, 3> max_vel = {0.5, 0.5, 0.5};
  const std::string pose_frame = "base_link";
  const bool enable_collision_check = true;
  const bool is_blocking = true;
  const float timeout_s = 20.0f;
  const bool omni_plan = false;

  std::cout << "navigate_to_goal_v2 relative target in base_link frame" << std::endl;
  NavigationStatus status = navigation.navigate_to_goal_v2(
      relative_goal, max_vel, pose_frame, enable_collision_check, is_blocking, timeout_s, omni_plan);

  std::cout << "navigate_to_goal_v2 status: " << navigation_status_to_string(status) << std::endl;
  print_current_pose(navigation);

  navigation.stop_navigation();
  std::cout << "navigation stopped" << std::endl;

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();


  std::cout << "example completed ..." << std::endl;
  return 0;
}

速度导航（navigate_with_velocity）

适用场景：通过速度指令连续控制底盘平移和旋转。

examples/cpp/galbot_navigation/src/navigate_with_velocity_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

std::string navigation_status_to_string(NavigationStatus status) {
  switch (status) {
    case NavigationStatus::SUCCESS:
      return "SUCCESS";
    case NavigationStatus::FAIL:
      return "FAIL";
    case NavigationStatus::TIMEOUT:
      return "TIMEOUT";
    case NavigationStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case NavigationStatus::MODE_ERR:
      return "MODE_ERR";
    case NavigationStatus::COMM_ERR:
      return "COMM_ERR";
    case NavigationStatus::WAIT_INITIALIZED:
      return "WAIT_INITIALIZED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
  Pose current_pose = navigation.get_current_pose();
  std::cout << indent << "current_pose: [" << current_pose.position.x << ", " << current_pose.position.y << ", "
            << current_pose.position.z << ", " << current_pose.orientation.x << ", " << current_pose.orientation.y
            << ", " << current_pose.orientation.z << ", " << current_pose.orientation.w << "]" << std::endl;
}

}  // namespace

int test_base_vel_cmd() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  auto controller_status = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(3000));
  print_current_pose(navigation);

  const double vx = 0.3;
  const double vy = 0.0;
  const double vyaw = 0.0;
  const double duration_s = 6.0;
  const bool enable_collision_check = true;

  std::cout << "navigate_with_velocity: vx=" << vx << ", vy=" << vy << ", vyaw=" << vyaw
            << ", duration_s=" << duration_s << std::endl;

  NavigationStatus status = navigation.navigate_with_velocity(vx, vy, vyaw, duration_s, enable_collision_check);

  std::cout << "navigate_with_velocity status: " << navigation_status_to_string(status) << std::endl;

  std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>((duration_s + 2.0) * 1000)));

  navigation.stop_navigation();
  print_current_pose(navigation);
  std::cout << "navigation stopped" << std::endl;

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  std::cout << "example completed ..." << std::endl;
  return 0;
}

int test_change_vel_cmd() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  auto controller_status = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(3000));
  print_current_pose(navigation);

  {
    double vx = 0.3;
    double vy = 0.0;
    double vyaw = 0.0;
    double duration_s = 6.0;
    bool enable_collision_check = true;

    std::cout << "navigate_with_velocity: vx=" << vx << ", vy=" << vy << ", vyaw=" << vyaw
              << ", duration_s=" << duration_s << std::endl;
    NavigationStatus status = navigation.navigate_with_velocity(vx, vy, vyaw, duration_s, enable_collision_check);
    std::cout << "navigate_with_velocity status 1: " << navigation_status_to_string(status) << std::endl;

    std::this_thread::sleep_for(std::chrono::milliseconds(4000));
  }

  std::cout << "已经运行4s 现在开始下发新的速度指令 =======" << std::endl;
  {
    double vx = 0.1;
    double vy = 0.2;
    double vyaw = 0.0;
    double duration_s = 6.0;
    bool enable_collision_check = true;

    std::cout << "navigate_with_velocity: vx=" << vx << ", vy=" << vy << ", vyaw=" << vyaw
              << ", duration_s=" << duration_s << std::endl;
    NavigationStatus status = navigation.navigate_with_velocity(vx, vy, vyaw, duration_s, enable_collision_check);
    std::cout << "navigate_with_velocity status 2: " << navigation_status_to_string(status) << std::endl;

    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>((duration_s + 2.0) * 1000)));
  }

  navigation.stop_navigation();
  print_current_pose(navigation);
  std::cout << "navigation stopped" << std::endl;

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  std::cout << "example completed ..." << std::endl;
  return 0;
}

// 根据选择放开不同的注释
int main() {
  test_base_vel_cmd();  // 测试速度导航接口基础功能

  //   test_change_vel_cmd();// 测试中途速度变化命令下发后的导航运动功能

  return 0;
}

导航配置

适用场景：读取和更新导航模块使用的运行配置。

examples/cpp/galbot_navigation/src/navigation_set_get_config_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <array>
#include <iostream>
#include <string>

using namespace galbot::sdk;

namespace {

std::string navigation_status_to_string(NavigationStatus status) {
  switch (status) {
    case NavigationStatus::SUCCESS:
      return "SUCCESS";
    case NavigationStatus::FAIL:
      return "FAIL";
    case NavigationStatus::TIMEOUT:
      return "TIMEOUT";
    case NavigationStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case NavigationStatus::MODE_ERR:
      return "MODE_ERR";
    case NavigationStatus::COMM_ERR:
      return "COMM_ERR";
    case NavigationStatus::WAIT_INITIALIZED:
      return "WAIT_INITIALIZED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_status(const std::string& name, NavigationStatus status) {
  std::cout << name << ": " << navigation_status_to_string(status) << std::endl;
}

}  // namespace

int main() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  std::cout << "dump navigation configs before setting parameters" << std::endl;
  print_status("dump_navigation_configs", navigation.dump_navigation_configs());

  const std::array<double, 3> vel_limit = {0.5, 0.5, 0.5};       // valid range: [0.05, 1.5]
  const std::array<double, 3> acc_limit = {0.5, 0.5, 0.5};       // valid range: [0.05, 6.0]
  const std::array<double, 3> jerk_limit = {5.0, 5.0, 5.0};      // valid range: [0.05, 12.0]
  const std::array<double, 3> arrival_threshold = {0.05, 0.05, 0.05};  // valid range: [0.03, 2.0]
  const double timeout_s = 30.0;

  print_status("set_navigation_velocity_limit",
               navigation.set_navigation_velocity_limit(vel_limit));

  print_status("set_navigation_kinematics_limits",
               navigation.set_navigation_kinematics_limits(vel_limit, acc_limit, jerk_limit));

  print_status("set_navigation_timeout",
               navigation.set_navigation_timeout(timeout_s));

  print_status("set_navigation_arrival_threshold",
               navigation.set_navigation_arrival_threshold(arrival_threshold));

  std::cout << "dump navigation configs after setting parameters" << std::endl;
  print_status("dump_navigation_configs", navigation.dump_navigation_configs());

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

多路点导航（navigation_through_waypoints）

适用场景：让机器人按照指定顺序依次访问多个导航路点。

examples/cpp/galbot_navigation/src/navigation_through_waypoints_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <thread>
#include <chrono>
#include <vector>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;
constexpr int kPollIntervalMs = 1000;
constexpr double kTaskTimeoutSec = 100.0;

std::string navigation_target_status_to_string(NavigationTaskStatus status) {
    switch (status) {
        case NavigationTaskStatus::UNKNOWN:
            return "UNKNOWN";
        case NavigationTaskStatus::RUNNING:
            return "RUNNING";
        case NavigationTaskStatus::SUCCESS:
            return "SUCCESS";
        case NavigationTaskStatus::FAILED:
            return "FAILED";
        case NavigationTaskStatus::INTERRUPTED:
            return "INTERRUPTED";
        case NavigationTaskStatus::OCCUPIED:
            return "OCCUPIED";
        case NavigationTaskStatus::COLLISION:
            return "COLLISION";
        case NavigationTaskStatus::CLOSE_TO_OBSTACLE:
            return "CLOSE_TO_OBSTACLE";
        default:
            return "UNKNOWN";
    }
}

bool is_terminal_status(NavigationTaskStatus status) {
    return status == NavigationTaskStatus::SUCCESS || status == NavigationTaskStatus::FAILED ||
           status == NavigationTaskStatus::INTERRUPTED || status == NavigationTaskStatus::OCCUPIED ||
           status == NavigationTaskStatus::COLLISION || status == NavigationTaskStatus::CLOSE_TO_OBSTACLE;
}

void print_task_snapshot(const TaskHandle& handle, const NavigationTaskSnapshot& snapshot) {
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
    Pose current_pose = navigation.get_current_pose();
    std::cout << indent << "current_pose: ["
              << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
              << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
              << current_pose.orientation.w << "]" << std::endl;
}

void monitor_task_until_terminal(GalbotNavigation& navigation, const TaskHandle& handle, double timeout_s) {
    const auto start = std::chrono::steady_clock::now();
    while (true) {
        NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
        print_task_snapshot(handle, snapshot);
        print_current_pose(navigation, "    ");

        const auto now = std::chrono::steady_clock::now();
        const double elapsed = std::chrono::duration<double>(now - start).count();

        if (is_terminal_status(snapshot.status)) {
            return;
        }

        if (elapsed >= timeout_s) {
            std::cout << "  timeout reached while waiting for task completion." << std::endl;
            return;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(kPollIntervalMs));
    }
}

}  // namespace

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
    int retry = 0;
    while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
        ++retry;
    }
    return navigation.is_localized();
}

static Waypoint make_waypoint(const Pose& pose, const WaypointParams& params = WaypointParams()) {
    return Waypoint(pose, params);
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
    auto& robot = GalbotRobot::get_instance(MachineType::G1);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    /* All coordinates are in the "map" frame. */
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    /*
     * Build a waypoint by combining:
     *   1) the target pose
     *   2) an optional WaypointParams object
     *
     * Usage:
     * - Use Waypoint(pose) if you only need the default settings.
     * - Create WaypointParams and set only the fields you want to override.
     * - Pass the params object into the Waypoint constructor to build a target point.
     */
    WaypointParams waypoint_2_params;
    waypoint_2_params.arrival_position_threshold_x = 0.02;
    waypoint_2_params.arrival_position_threshold_y = 0.02;
    waypoint_2_params.arrival_orientation_threshold = 0.08;
    waypoint_2_params.velocity_scale = 0.2;

    WaypointParams waypoint_3_params;
    waypoint_3_params.arrival_position_threshold_x = 0.04;
    waypoint_3_params.arrival_position_threshold_y = 0.04;
    waypoint_3_params.arrival_orientation_threshold = 0.05;
    waypoint_3_params.velocity_scale = 0.8;
    waypoint_3_params.acceleration_scale = 0.8;
    waypoint_3_params.jerk_scale = 0.8;

    std::vector<Waypoint> waypoints = {
        make_waypoint(Pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0})),
        make_waypoint(Pose(std::vector<double>{0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}), waypoint_2_params),
        make_waypoint(Pose(std::vector<double>{1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}), waypoint_3_params)
    };

    /* Wait for data preparation */
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    if (!wait_for_localization(navigation, init_pose)) {
        std::cerr << "localization failed" << std::endl;
        robot.request_shutdown();
        robot.wait_for_shutdown();
        robot.destroy();
        return -1;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    std::cout << "set target: waypoints" << std::endl;
    for (size_t i = 0; i < waypoints.size(); ++i) {
        std::cout << "  waypoint_" << i << ": ["
                  << waypoints[i].pose.position.x << ", " << waypoints[i].pose.position.y << ", " << waypoints[i].pose.position.z << ", "
                  << waypoints[i].pose.orientation.x << ", " << waypoints[i].pose.orientation.y << ", " << waypoints[i].pose.orientation.z << ", "
                  << waypoints[i].pose.orientation.w << "]" << std::endl;
    }

    // The reference frame of the trajectory (can be set to "map" or "base_link", Defaulting to "map")
    const std::string frame_id = "map";
    // Whether to enable collision checking (can be set to true in open environments)
    bool enable_collision_check = true;

    TaskHandle handle = navigation.navigate_through_waypoints(waypoints, frame_id, enable_collision_check);

    if (handle.request_sent) {
        monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec);
    } else {
        std::cout << "navigation failed to submit task" << std::endl;
    }

    std::cout << "\nfinal task summary" << std::endl;
    NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;

    // Stop navigation
    navigation.stop_navigation();

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

设置导航目标（set_navigation_target）

适用场景：在启动或管理导航任务前，单独设置导航目标。

examples/cpp/galbot_navigation/src/set_navigation_target_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;
constexpr int kPollIntervalMs = 500;
constexpr int kMonitorDurationMs = 2000;

std::string navigation_target_status_to_string(NavigationTaskStatus status) {
  switch (status) {
    case NavigationTaskStatus::UNKNOWN:
      return "UNKNOWN";
    case NavigationTaskStatus::RUNNING:
      return "RUNNING";
    case NavigationTaskStatus::SUCCESS:
      return "SUCCESS";
    case NavigationTaskStatus::FAILED:
      return "FAILED";
    case NavigationTaskStatus::INTERRUPTED:
      return "INTERRUPTED";
    case NavigationTaskStatus::OCCUPIED:
      return "OCCUPIED";
    case NavigationTaskStatus::COLLISION:
      return "COLLISION";
    case NavigationTaskStatus::CLOSE_TO_OBSTACLE:
      return "CLOSE_TO_OBSTACLE";
    default:
      return "UNKNOWN";
  }
}


void print_pose(const Pose& pose, const std::string& label) {
  std::cout << label << ": ["
            << pose.position.x << ", " << pose.position.y << ", " << pose.position.z << ", "
            << pose.orientation.x << ", " << pose.orientation.y << ", " << pose.orientation.z << ", "
            << pose.orientation.w << "]" << std::endl;
}

void print_task_snapshot(const TaskHandle& handle, const NavigationTaskSnapshot& snapshot) {
  std::cout << "  task_id: " << handle.task_id
            << ", request_sent: " << (handle.request_sent ? "true" : "false")
            << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
  Pose current_pose = navigation.get_current_pose();
  std::cout << indent << "current_pose: ["
            << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
            << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
            << current_pose.orientation.w << "]" << std::endl;
}

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
  int retry = 0;
  while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
    navigation.relocalize(init_pose);
    std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
    ++retry;
  }
  return navigation.is_localized();
}

void print_task_summary(GalbotNavigation& navigation, const std::vector<TaskHandle>& handles) {
  std::cout << "\nfinal task summary" << std::endl;
  for (const auto& task : handles) {
    const NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(task.task_id);
    print_task_snapshot(task, snapshot);
  }
}


}  // namespace

int main() {

  auto& navigation = GalbotNavigation::get_instance(MachineType::G1);
  auto& robot = GalbotRobot::get_instance(MachineType::G1);

  // Initialize system
  if (robot.init()) {
      std::cout << "Base instance initialized successfully!" << std::endl;
  } else {
      std::cerr << "Base instance initialization failed!" << std::endl;
      return -1;
  }
  if (navigation.init()) {
      std::cout << "Navigation instance initialized successfully!" << std::endl;
  } else {
      std::cerr << "Navigation instance initialization failed!" << std::endl;
      return -1;
  }
  auto controller_status = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

  /* Wait for data preparation */
  std::this_thread::sleep_for(std::chrono::milliseconds(3000)); 

  if (!wait_for_localization(navigation, init_pose)) {
    std::cerr << "localization failed" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const std::string frame_id = "map";
  float speed_ratio = 0.6f;
  bool enable_collision_check = true;
  const std::vector<Pose> target_points = {
      Pose(std::vector<double>{1.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00}),
      Pose(std::vector<double>{0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00}),
      Pose(std::vector<double>{0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00})
  };

  std::vector<TaskHandle> handles;
  handles.reserve(target_points.size());

  for (size_t i = 0; i < target_points.size(); ++i) {
    std::cout << "set target: dynamic_goal_" << i << ", target_pose: ["
              << target_points[i].position.x << ", " << target_points[i].position.y << ", " << target_points[i].position.z << ", "
              << target_points[i].orientation.x << ", " << target_points[i].orientation.y << ", " << target_points[i].orientation.z << ", "
              << target_points[i].orientation.w << "]" << std::endl;

    TaskHandle handle =
        navigation.set_navigation_target(target_points[i], frame_id, speed_ratio, enable_collision_check);

    handles.push_back(handle);

    const auto start = std::chrono::steady_clock::now();
    while (true) {
      NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
      print_task_snapshot(handle, snapshot);
      print_current_pose(navigation, "    ");

      const auto now = std::chrono::steady_clock::now();
      const auto elapsed_ms =
          std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
      if (elapsed_ms >= kMonitorDurationMs) {
        break;
      }

      std::this_thread::sleep_for(std::chrono::milliseconds(kPollIntervalMs));
    }
  }

  print_task_summary(navigation, handles);

  std::cout << "\nstop navigation and shutdown" << std::endl;
  navigation.stop_navigation();

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

类：GalbotPerception

高精度双目深度估计，单次推理并保存伪彩色深度图

适用场景：触发一次双目深度估计推理，并获取推理结果

examples/cpp/galbot_perception/src/foundation_stereo_run_once_example.cpp

/**
 * Foundation stereo depth example: single run_once + wait_for_new_result to fetch one inference result.
 */

#include <iostream>
#include <thread>
#include <chrono>

#include "galbot_robot.hpp"
#include "galbot_perception.hpp"
#include "galbot_sdk_type.hpp"
#include "opencv2/opencv.hpp"

using namespace galbot::sdk;

bool run_foundation_stereo_once(GalbotPerception& perception) {
    if (!perception.run_once(PerceptionModule::FOUNDATION_STEREO)) {
        std::cerr << "run_once failed to send command" << std::endl;
        return false;
    }

    std::cout << "Waiting for inference result..." << std::endl;
    if (!perception.wait_for_new_result(PerceptionModule::FOUNDATION_STEREO, 5.0)) {
        std::cerr << "Timed out waiting for inference result" << std::endl;
        return false;
    }

    DetectionResult result;
    if (!perception.get_latest_result(PerceptionModule::FOUNDATION_STEREO, result)) {
        std::cerr << "get_latest_result failed" << std::endl;
        return false;
    }

    if (!result.instanceMask.empty()) {
        cv::Mat depth_map = result.instanceMask;
        std::cout << "Depth map size: " << depth_map.cols << "x" << depth_map.rows
                << ", type: " << depth_map.type() << std::endl;

        double min_val, max_val;
        cv::minMaxLoc(depth_map, &min_val, &max_val);
        std::cout << "Depth value range: [" << min_val << ", " << max_val << "]" << std::endl;

        cv::Mat depth_f;
        depth_map.convertTo(depth_f, CV_32F);
        cv::Mat mask = (depth_f > 0);

        cv::Mat normalized;
        cv::normalize(depth_f, normalized, 0, 255, cv::NORM_MINMAX, CV_8UC1, mask);

        cv::Mat colored;
        cv::applyColorMap(normalized, colored, cv::COLORMAP_TURBO);

        cv::imwrite("foundation_stereo_depth.jpg", colored);
        std::cout << "Depth map saved to foundation_stereo_depth.jpg" << std::endl;
    } else {
        std::cout << "No depth map (instanceMask is empty)" << std::endl;
    }

    return true;
}

int main() {
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    robot.init();

    auto& perception = GalbotPerception::get_instance(MachineType::G1);
    perception.init({PerceptionModule::FOUNDATION_STEREO});

    // Wait for perception models to load
    std::this_thread::sleep_for(std::chrono::seconds(12));
    std::cout << "Init OK, sending single inference request..." << std::endl;

    run_foundation_stereo_once(perception);

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

类：Parameter

创建并使用 Parameter(parameter_use)

适用场景：创建运动规划参数对象，用于配置运动规划的各种参数如速度限制、加速度限制、规划时间等。

examples/cpp/galbot_motion/src/parameter_use_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <memory>

#include "galbot_motion.hpp"

using namespace galbot::sdk;

int main() {
    // Create Parameter via constructor and set options
    auto p = std::make_shared<Parameter>();

    p->set_blocking(true);            // Set whether to block execution
    p->set_check_collision(false);     // Disable collision detection
    p->set_timeout(5.0);              // Set timeout (seconds)
    p->set_actuate("with_chain_only");// Set drive mode
    p->set_tool_pose(false);           // Whether to consider tool pose
    p->set_reference_frame("base_link");

    std::cout << "--- Parameter p ---" << std::endl;
    std::cout << "blocking: " << (p->get_blocking() ? "True" : "False") << std::endl;
    std::cout << "collision check: " << (p->get_check_collision() ? "True" : "False") << std::endl;
    std::cout << "timeout: " << p->get_timeout() << "s" << std::endl;

    return 0;
}

辅助函数

status_to_string / JointStates / PoseState (auxi_fun_use)

适用场景：展示如何使用辅助函数转换状态为字符串，创建关节状态和位姿状态对象。

examples/cpp/galbot_motion/src/function_use_example.cpp

#include <iostream>
#include <string>

#include "galbot_motion.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G1);

    MotionStatus status = MotionStatus::SUCCESS;
    std::string status_str = planner.status_to_string(status);
    
    std::cout << "MotionStatus string: " << status_str << std::endl;

    auto js = std::make_shared<JointStates>();
    auto ps = std::make_shared<PoseState>();

    js->chain_name = "left_arm";
    js->joint_positions = std::vector<double>(7, 0.0); 

    ps->chain_name = "left_arm";
    ps->pose = Pose(std::vector<double>{1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    ps->reference_frame = "base_link"; 
    ps->frame_id = "EndEffector";

    std::cout << "--- JointStates ---" << std::endl;
    std::cout << "Type: " << typeid(*js).name() << std::endl; // Print class name
    std::cout << "Chain Name: " << js->chain_name << std::endl;
    std::cout << "Joints size: " << js->joint_positions.size() << std::endl;

    std::cout << "\n--- PoseState ---" << std::endl;
    std::cout << "Type: " << typeid(*ps).name() << std::endl;
    std::cout << "Chain Name: " << ps->chain_name << std::endl;
    std::cout << "Pose Z: " << ps->pose.position.z << std::endl;

    return 0;
}

教程源码示例

以下可运行教程维护在 examples/g1/cpp/tutorials 中；使用步骤与安全说明请参阅入门教程。

示例 1：Galbot 控制入门

适用场景：G1 最基础的关节控制：执行双臂比心动作并恢复初始姿态。运行前请确认机器人处于工作模式且急停按钮已旋开；夹爪控制及更多控制案例请参阅本页 galbot_robot 下的示例。

examples/cpp/tutorials/src/example1_galbot_control_started.cpp

#include<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void demo_heart_pose(GalbotRobot& robot,
                     const std::vector<std::string>& joint_group_names,
                     const std::vector<std::vector<double>>& position_seq,
                     bool is_blocking, double max_speed, double timeout_s){
    /** Get current joint group angles for subsequent restoration */
    std::vector<double> original_pos = robot.get_joint_positions(joint_group_names, {});
    for (auto a: original_pos){
        std::cout << "Current angles of joint group " << a << std::endl;
    }

    /** Execute heart pose sequence */
    int pos_idx = 0;
    std::cout << "Starting heart gesture..." << std::endl;
    while (pos_idx < position_seq.size()) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        std::vector<double> pos = position_seq[pos_idx];
        ControlStatus control_status = robot.set_joint_positions(
            pos, joint_group_names, {}, is_blocking, max_speed, timeout_s);
        if (control_status == ControlStatus::SUCCESS) {
            pos_idx++;
        } else {
            std::cerr << "Failed to set joint positions." << std::endl;
            break;
        }
    }

    /** Restore original joint positions */
    robot.set_joint_positions(original_pos, joint_group_names, {}, is_blocking, max_speed, timeout_s);
    std::cout << "Restored joint positions of joint group " << std::endl;
    
}

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G1); 

        /* Initialize robot */
        if (robot.init()) {
            std::cout << "Initialization successful" << std::endl;
            std::cout << "Is robot running: " << robot.is_running() << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        
        /** Get joint names */
        std::vector<std::string> joint_names = robot.get_joint_names(true, {"leg", "head", "left_arm", "right_arm"});
        if (joint_names.empty()) {
            std::cerr << "No joint names available!" << std::endl;
        } else {
            for (const auto& name : joint_names) {
                std::cout << name << std::endl;
            }
        }

        /** Get joint positions using joint group names, empty returns all joints by default */
        std::vector<std::string> joint_group_names = {"left_arm", "right_arm"};
        std::vector<std::vector<double>> position_seq = {{1.53, 0.36, -2.54, -1.80, 0.12, -0.82, 0.09,
             -1.53, -0.36, 2.54, 1.80, -0.12, 0.82, -0.09}}; // right_arm
        bool is_blocking = true;
        double max_speed = 0.1;
        double timeout_s = 20;

        demo_heart_pose(robot, joint_group_names, position_seq,
                        is_blocking, max_speed, timeout_s);


        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    return 0;
}

示例 2：机械臂操作

适用场景：学习机械臂正逆运动学，并使用规划结果控制机械臂。

examples/cpp/tutorials/src/example2_arm_manipulation.cpp

#include<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include <array>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"
#include "galbot_motion.hpp"


using namespace galbot::sdk;

inline std::vector<double> quat_normalize(const std::vector<double>& q)
{
    double norm = std::sqrt(
        q[0]*q[0] + q[1]*q[1] +
        q[2]*q[2] + q[3]*q[3]
    );
    if (norm <= 0.0)
        throw std::runtime_error("Zero-norm quaternion");

    return std::vector<double>{ q[0]/norm, q[1]/norm, q[2]/norm, q[3]/norm };
}
inline std::vector<double> quat_conjugate(const std::vector<double>& q)
{
    return std::vector<double>{ -q[0], -q[1], -q[2], q[3] };
}

inline std::vector<double> quat_multiply(const std::vector<double>& q1, const std::vector<double>& q2)
{
    const double x1 = q1[0];
    const double y1 = q1[1];
    const double z1 = q1[2];
    const double w1 = q1[3];
    const double x2 = q2[0];
    const double y2 = q2[1];
    const double z2 = q2[2];
    const double w2 = q2[3];

    return std::vector<double>{
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2,
        w1*w2 - x1*x2 - y1*y2 - z1*z2
    };
}

template <typename T>
inline T clamp(const T& v, const T& lo, const T& hi)
{
    return (v < lo) ? lo : (v > hi) ? hi : v;
}

inline double orientation_error_angle(const std::vector<double>& A, const std::vector<double>& B)
{
    std::vector<double> qA = quat_normalize({ A[3], A[4], A[5], A[6] });
    std::vector<double> qB = quat_normalize({ B[3], B[4], B[5], B[6] });

    std::vector<double> q_err = quat_multiply(qB, quat_conjugate(qA));
    q_err = quat_normalize(q_err);

    // Numerically stable
    double qw = clamp(q_err[3], -1.0, 1.0);

    return 2.0 * std::acos(qw);
}

struct PoseError
{
    double position_error_norm;   // meters
    double orientation_error_rad;
    double orientation_error_deg;
};

inline PoseError calculate_error(const std::vector<double>& pose1, const std::vector<double>& pose2)
{
    double dx = pose1[0] - pose2[0];
    double dy = pose1[1] - pose2[1];
    double dz = pose1[2] - pose2[2];

    double pos_err = std::sqrt(dx*dx + dy*dy + dz*dz);
    double rot_err = orientation_error_angle(pose1, pose2);

    return PoseError{
        pos_err,
        rot_err,
        rot_err * 180.0 / M_PI
    };
}

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G1); 
        auto& motion = GalbotMotion::get_instance(MachineType::G1); 

        /* Initialize robot */
        if (robot.init()) {
            std::cout << "Initialization successful" << std::endl;
            std::cout << "Is robot running: " << robot.is_running() << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
        }
        
        /** Initialize motion */
        if (motion.init()) {
            std::cout << "Motion initialization successful" << std::endl;
        }else{
            std::cerr << "Motion initialization failed" << std::endl;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));

        // set initial joint positions
        std::vector<std::string> joint_groups = {"leg", "head", "left_arm", "right_arm"};
        std::vector<std::string> joint_names = {};

        std::vector<double> joint_pos = {0.5, 1.5, 1.0, 0.0, 0.0, 
            0.0, 0.0,
            2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
            -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0};
        bool is_block = true;
        double max_speed_rad_s = 0.1;
        double timeout_s = 30.0;

        galbot::sdk::ControlStatus joint_execution_status =
            robot.set_joint_positions(joint_pos, joint_groups, joint_names, is_block, max_speed_rad_s, timeout_s);

        if (joint_execution_status == ControlStatus::SUCCESS) {
            std::cout << "✅ Initial joint positions set successfully!" << std::endl;
        } else {
            std::cerr << "Failed to set initial joint positions!" << std::endl;
        }
        
        /** Define target pose */
        std::map<std::string, std::vector<double>> chain_pose_baselink = {
            {"leg", {0.0596,-0.0000,1.0327,0.5000,0.5003,0.4997,0.5000}},
            {"head", {0.0599,0.0002,1.4098,-0.7072,0.0037,0.0037,0.7069}},
            {"left_arm", {0.1267,0.2342,0.7356,0.0220,0.0127,0.0343,0.9991}},
            {"right_arm", {0.1267,-0.2345,0.7358,-0.0225,0.0126,-0.0343,0.9991}}
        };

        std::string target_chain = "left_arm";
        std::string reference_frame = "base_link";
        std::string target_frame = "EndEffector";
        std::string end_link = "left_arm_end_effector_mount_link";

        /** 1. Get end effector pose on chain */
        std::tuple<MotionStatus, std::vector<double>> ret = motion.get_end_effector_pose_on_chain(
            target_chain, target_frame, reference_frame
        );
        if(std::get<0>(ret) == MotionStatus::SUCCESS){
            std::cout << "✅ Current " << target_chain << "end-effector pose: " << std::get<1>(ret).size() << std::endl;
            for (auto pose : std::get<1>(ret)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;
        }else{
            std::cerr << "Failed to get end effector pose on chain!" << std::endl;
        }

        /** 2. Solve joint angles based on target pose IK and verify the solution 
         *  2.1 Solve joint angles joint_angles_ik for target pose through IK
        */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        bool enable_collision_check = false;
        std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<double>>> ret_ik = motion.inverse_kinematics(
            chain_pose_baselink[target_chain],
            {target_chain},
            target_frame, 
            reference_frame,
            {},
            enable_collision_check,
            std::make_shared<Parameter>()
        );
        if (std::get<0>(ret_ik) == MotionStatus::SUCCESS){
            std::cout << "✅ Target" << target_chain << "IK solving successful joint_angles_ik:" << std::endl;
            for (auto joint_angle : std::get<1>(ret_ik)[target_chain]){
                std::cout << joint_angle << " ";
            }
            std::cout << std::endl;
        }else{
            std::cerr << "IK solving failed" << std::endl;
        }

        /** 2.2 Set end-effector pose to target pose tgt_pose_ik by setting 
         * joint group angles joint_angles_ik 
        */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        ControlStatus ret2 = robot.set_joint_positions(
            std::get<1>(ret_ik)[target_chain],
            {target_chain},
            {},
            true,
            0.1,
            20.0
        );
        if(ret2 == ControlStatus::SUCCESS){
            std::cout << "✅ Target " << target_chain << "pose setting successful" << std::endl;
        }else{
            std::cerr << "Target " << target_chain << "pose setting failed" << std::endl;
        }

        /** 2.3 Verify whether the set joint group angles are consistent with the solved angles */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        std::tuple<MotionStatus, std::vector<double>> ret3 = motion.get_end_effector_pose_on_chain(
            target_chain,
            target_frame,
            reference_frame
        );
        if(std::get<0>(ret3) == MotionStatus::SUCCESS){
            std::cout << "✅ Verified " << target_chain << "end-effector pose: " << std::get<1>(ret3).size() << std::endl;
            for (auto pose : std::get<1>(ret3)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;
        }else{
            std::cerr << "Failed to verify end effector pose on chain!" << std::endl;
        }
        
        /** 2.4 Verify whether the end-effector pose tgt_pose_fk corresponding to joint group angles joint_angles_ik solved by FK is consistent with target pose tgt_pose_ik */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        std::tuple<MotionStatus, std::vector<double>> ret_fk = motion.forward_kinematics(
            end_link,
            reference_frame,
            std::get<1>(ret_ik),
            std::make_shared<Parameter>()
        );
        if (std::get<0>(ret_fk) == MotionStatus::SUCCESS){
            std::cout << "✅ Verified " << target_chain << "end-effector pose: " << std::get<1>(ret_fk).size() << std::endl;
            for (auto pose : std::get<1>(ret_fk)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;

            PoseError err = calculate_error(chain_pose_baselink[target_chain], std::get<1>(ret_fk));
            std::cout << "Position error: " << err.position_error_norm << " m" << std::endl;
            std::cout << "Orientation error: " << err.orientation_error_deg << " deg" << std::endl;
        }else{
            std::cerr << "Failed to verify end effector pose on chain!" << std::endl;
        }

        /** 3. Restore to original pose by setting end-effector pose
         *  3.1 Set end-effector pose to restore to original pose
         */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        MotionStatus ret4 = motion.set_end_effector_pose(
            std::get<1>(ret),
            target_chain,
            reference_frame,
            nullptr,
            enable_collision_check,
            true,
            5.0,
            std::make_shared<Parameter>()
        );
        if(ret4 == MotionStatus::SUCCESS){
            std::cout << "✅ Restored " << target_chain << "end-effector pose to original pose" << std::endl;
        }else{
            std::cerr << "Failed to restore " << target_chain << "end-effector pose to original pose" << std::endl;
        }

        /** 3.2 Get end-effector pose and verify whether it has been restored to original pose */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        std::tuple<MotionStatus, std::vector<double>> ret5 = motion.get_end_effector_pose_on_chain(
            target_chain,
            target_frame,
            reference_frame
        );
        if(std::get<0>(ret5) == MotionStatus::SUCCESS){
            std::cout << "✅ Verified " << target_chain << "end-effector pose: " << std::get<1>(ret5).size() << std::endl;
            for (auto pose : std::get<1>(ret5)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;

            PoseError err = calculate_error(std::get<1>(ret), std::get<1>(ret5));
            std::cout << "Position error: " << err.position_error_norm << " m" << std::endl;
            std::cout << "Orientation error: " << err.orientation_error_deg << " deg" << std::endl;
        }else{
            std::cerr << "Failed to verify end effector pose on chain!" << std::endl;
        }

        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    return 0;
}

示例 3：机器人导航

适用场景：初始化导航、完成机器人定位并导航至目标位姿。

examples/cpp/tutorials/src/example3_robot_navigation.cpp

#include<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include <array>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"
#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"

using namespace galbot::sdk;

inline std::vector<double> quat_normalize(const std::vector<double>& q)
{
    double norm = std::sqrt(
        q[0]*q[0] + q[1]*q[1] +
        q[2]*q[2] + q[3]*q[3]
    );
    if (norm <= 0.0)
        throw std::runtime_error("Zero-norm quaternion");

    return std::vector<double>{ q[0]/norm, q[1]/norm, q[2]/norm, q[3]/norm };
}
inline std::vector<double> quat_conjugate(const std::vector<double>& q)
{
    return std::vector<double>{ -q[0], -q[1], -q[2], q[3] };
}

inline std::vector<double> quat_multiply(const std::vector<double>& q1, const std::vector<double>& q2)
{
    const double x1 = q1[0];
    const double y1 = q1[1];
    const double z1 = q1[2];
    const double w1 = q1[3];
    const double x2 = q2[0];
    const double y2 = q2[1];
    const double z2 = q2[2];
    const double w2 = q2[3];

    return std::vector<double>{
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2,
        w1*w2 - x1*x2 - y1*y2 - z1*z2
    };
}

template <typename T>
inline T clamp(const T& v, const T& lo, const T& hi)
{
    return (v < lo) ? lo : (v > hi) ? hi : v;
}

inline double orientation_error_angle(const std::vector<double>& A, const std::vector<double>& B)
{
    std::vector<double> qA = quat_normalize({ A[3], A[4], A[5], A[6] });
    std::vector<double> qB = quat_normalize({ B[3], B[4], B[5], B[6] });

    std::vector<double> q_err = quat_multiply(qB, quat_conjugate(qA));
    q_err = quat_normalize(q_err);

    // Numerically stable
    double qw = clamp(q_err[3], -1.0, 1.0);

    return 2.0 * std::acos(qw);
}

struct PoseError
{
    double position_error_norm;   // meters
    double orientation_error_rad;
    double orientation_error_deg;
};

inline PoseError calculate_error(const std::vector<double>& pose1, const std::vector<double>& pose2)
{
    double dx = pose1[0] - pose2[0];
    double dy = pose1[1] - pose2[1];
    double dz = pose1[2] - pose2[2];

    double pos_err = std::sqrt(dx*dx + dy*dy + dz*dz);
    double rot_err = orientation_error_angle(pose1, pose2);

    return PoseError{
        pos_err,
        rot_err,
        rot_err * 180.0 / M_PI
    };
}


/**
 * @brief Convert quaternion [x, y, z, w] to 3x3 rotation matrix
 * @param qx X component of quaternion
 * @param qy Y component of quaternion
 * @param qz Z component of quaternion
 * @param qw W component (scalar) of quaternion
 * @return 3x3 rotation matrix (CV_64F)
 */
inline cv::Mat quat_to_rotation_matrix(double qx, double qy, double qz, double qw) {
    cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
    
    R.at<double>(0, 0) = 1 - 2*(qy*qy + qz*qz);
    R.at<double>(0, 1) = 2*(qx*qy - qw*qz);
    R.at<double>(0, 2) = 2*(qx*qz + qw*qy);
    
    R.at<double>(1, 0) = 2*(qx*qy + qw*qz);
    R.at<double>(1, 1) = 1 - 2*(qx*qx + qz*qz);
    R.at<double>(1, 2) = 2*(qy*qz - qw*qx);
    
    R.at<double>(2, 0) = 2*(qx*qz - qw*qy);
    R.at<double>(2, 1) = 2*(qy*qz + qw*qx);
    R.at<double>(2, 2) = 1 - 2*(qx*qx + qy*qy);
    
    return R;
}

/**
 * @brief Convert 3x3 rotation matrix to quaternion [x, y, z, w]
 * @param R 3x3 rotation matrix (CV_64F)
 * @return Quaternion as vector [qx, qy, qz, qw]
 */
inline std::vector<double> rotation_matrix_to_quat(const cv::Mat& R) {
    double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2);
    double qx, qy, qz, qw;
    
    if (trace > 0) {
        double s = 0.5 / std::sqrt(trace + 1.0);
        qw = 0.25 / s;
        qx = (R.at<double>(2, 1) - R.at<double>(1, 2)) * s;
        qy = (R.at<double>(0, 2) - R.at<double>(2, 0)) * s;
        qz = (R.at<double>(1, 0) - R.at<double>(0, 1)) * s;
    } else if ((R.at<double>(0, 0) > R.at<double>(1, 1)) && (R.at<double>(0, 0) > R.at<double>(2, 2))) {
        double s = 2.0 * std::sqrt(1.0 + R.at<double>(0, 0) - R.at<double>(1, 1) - R.at<double>(2, 2));
        qw = (R.at<double>(2, 1) - R.at<double>(1, 2)) / s;
        qx = 0.25 * s;
        qy = (R.at<double>(0, 1) + R.at<double>(1, 0)) / s;
        qz = (R.at<double>(0, 2) + R.at<double>(2, 0)) / s;
    } else if (R.at<double>(1, 1) > R.at<double>(2, 2)) {
        double s = 2.0 * std::sqrt(1.0 + R.at<double>(1, 1) - R.at<double>(0, 0) - R.at<double>(2, 2));
        qw = (R.at<double>(0, 2) - R.at<double>(2, 0)) / s;
        qx = (R.at<double>(0, 1) + R.at<double>(1, 0)) / s;
        qy = 0.25 * s;
        qz = (R.at<double>(1, 2) + R.at<double>(2, 1)) / s;
    } else {
        double s = 2.0 * std::sqrt(1.0 + R.at<double>(2, 2) - R.at<double>(0, 0) - R.at<double>(1, 1));
        qw = (R.at<double>(1, 0) - R.at<double>(0, 1)) / s;
        qx = (R.at<double>(0, 2) + R.at<double>(2, 0)) / s;
        qy = (R.at<double>(1, 2) + R.at<double>(2, 1)) / s;
        qz = 0.25 * s;
    }
    
    return std::vector<double>{qx, qy, qz, qw};
}

/**
 * @brief Convert local pose to global pose
 * 
 * Transforms a pose from local frame to global frame using the start pose as reference.
 * Mathematically: global_pose = start_pose @ local_pose (matrix multiplication)
 * 
 * @param cur_pose Current/start pose (reference frame), [x, y, z, qx, qy, qz, qw]
 * @param local_pose Local pose relative to start, [x, y, z, qx, qy, qz, qw]
 * @return Global pose result, [x, y, z, qx, qy, qz, qw]
 */
Pose local_pose_to_global(Pose cur_pose, Pose local_pose){
    // Create 4x4 transformation matrix from start pose
    cv::Mat T_start = cv::Mat::eye(4, 4, CV_64F);
    cv::Mat R_start = quat_to_rotation_matrix(cur_pose.orientation.x, cur_pose.orientation.y, 
                                               cur_pose.orientation.z, cur_pose.orientation.w);
    R_start.copyTo(T_start(cv::Rect(0, 0, 3, 3)));
    T_start.at<double>(0, 3) = cur_pose.position.x;
    T_start.at<double>(1, 3) = cur_pose.position.y;
    T_start.at<double>(2, 3) = cur_pose.position.z;
    
    // Create 4x4 transformation matrix from local pose
    cv::Mat T_local = cv::Mat::eye(4, 4, CV_64F);
    cv::Mat R_local = quat_to_rotation_matrix(local_pose.orientation.x, local_pose.orientation.y,
                                               local_pose.orientation.z, local_pose.orientation.w);
    R_local.copyTo(T_local(cv::Rect(0, 0, 3, 3)));
    T_local.at<double>(0, 3) = local_pose.position.x;
    T_local.at<double>(1, 3) = local_pose.position.y;
    T_local.at<double>(2, 3) = local_pose.position.z;
    
    // Global transformation = start transformation * local transformation
    cv::Mat T_global = T_start * T_local;
    
    // Extract position and orientation from result
    cv::Mat R_global = T_global(cv::Rect(0, 0, 3, 3));
    std::vector<double> quat_global = rotation_matrix_to_quat(R_global);
    
    Pose result;
    result.position.x = T_global.at<double>(0, 3);
    result.position.y = T_global.at<double>(1, 3);
    result.position.z = T_global.at<double>(2, 3);
    result.orientation.x = quat_global[0];
    result.orientation.y = quat_global[1];
    result.orientation.z = quat_global[2];
    result.orientation.w = quat_global[3];
    
    return result;
}

void demo_square_move(GalbotRobot& robot, GalbotNavigation& navi){
    Pose local_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.707, 0.707});

    for(int i=0; i<4; ++i){
        Pose current_pose = navi.get_current_pose();
        std::cout << "Current pose: [" 
                  << current_pose.position.x << ", "
                  << current_pose.position.y << ", "
                  << current_pose.position.z << ", "
                  << current_pose.orientation.x << ", "
                  << current_pose.orientation.y << ", "
                  << current_pose.orientation.z << ", "
                  << current_pose.orientation.w << "]" << std::endl;

        Pose goal_pose = local_pose_to_global(current_pose, local_pose);
        
        if(navi.check_path_reachability(goal_pose, current_pose)){
            int retry_cnt = 3;
            for(;;){
                // navi.move_to_goal(goal_pose);
                std::this_thread::sleep_for(std::chrono::seconds(1));
                navi.navigate_to_goal(goal_pose, true, true, 30);
                retry_cnt -= 1;
                if(navi.check_goal_arrival() || retry_cnt <= 0){
                    break;
                }else{
                    std::cout << "Navigation failed, retry count: " << retry_cnt << std::endl;
                }
            }
        }
    }
    Pose final_pose = navi.get_current_pose();
    std::cout << "Final pose: [" 
              << final_pose.position.x << ", "
              << final_pose.position.y << ", "
              << final_pose.position.z << ", "
              << final_pose.orientation.x << ", "
              << final_pose.orientation.y << ", "
              << final_pose.orientation.z << ", "
              << final_pose.orientation.w << "]" << std::endl;
}

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G1); 
        auto& navi = GalbotNavigation::get_instance(MachineType::G1);        

        /* Initialize robot */
        if (robot.init()) {
            std::cout << "Initialization successful" << std::endl;
            std::cout << "Is robot running: " << robot.is_running() << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
        }
        /** Initialize navigation */
        if (navi.init()) {
            std::cout << "Navigation initialization successful" << std::endl;
        }else{
            std::cerr << "Navigation initialization failed" << std::endl;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        
        /** If not localized, relocalize */
        bool is_localized = navi.is_localized();
        std::cout << "Is robot localized: " << is_localized << std::endl;
        while(!is_localized){
            NavigationStatus status = navi.relocalize(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
            std::this_thread::sleep_for(std::chrono::seconds(3));
            std::cout << "Waiting for localization..." << std::endl;
            is_localized = navi.is_localized();
        }

        /** If localized, move the robot */
        demo_square_move(robot, navi);

        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    return 0;
}

示例 4：传感器数据采集

适用场景：采集相机、深度、雷达、IMU 等机器人传感器数据。

examples/cpp/tutorials/src/example4_sensor_data_collect.cpp

#include<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"

using namespace galbot::sdk;

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

struct Point3D {
    float x, y, z;
    uint8_t intensity;
};

std::vector<Point3D> get_xyz_points(const std::shared_ptr<LidarData>& lidar_data, bool remove_nan = false){
    std::vector<Point3D> points;
    if (!lidar_data || lidar_data->data.empty()) return points;

    /** 1. lookup the offset of x, y, z field */
    auto off_x = -1, off_y = -1, off_z = -1;
    auto off_intensity = -1;
    std::cout << "off_x" << off_x << std::endl;
    for (const auto& f: lidar_data->fields) {
        if (f.name == "x") off_x = f.offset;
        else if (f.name == "y") off_y = f.offset;
        else if (f.name == "z") off_z = f.offset;
        else if (f.name == "reflectivity") off_intensity = f.offset;
        std::cout << f.name << " " << f.datatype << " ";
    }
    std::cout << "\n";

    if (off_x == -1 || off_y == -1 || off_z == -1) {
        std::cerr << "Error: the lidar data has no required x/y/z field." << std::endl;
        return points;
    }

    /** 2. read data directly */
    uint32_t num_points = lidar_data->width * lidar_data->height;
    points.reserve(num_points);
    
    const uint8_t* raw_data = lidar_data->data.data();
    const uint32_t point_step = lidar_data->point_step;

    for (uint32_t i=0; i<num_points; ++i) {
        /** get pointer for current point */
        const uint8_t* point_ptr = raw_data + (i * point_step);
        
        /** use reinterpret_cast convert pointer type to read memory */
        /** assume points in lidar data are float32, which is most common data type */
        float x = *reinterpret_cast<const float*>(point_ptr + off_x);
        float y = *reinterpret_cast<const float*>(point_ptr + off_y);
        float z = *reinterpret_cast<const float*>(point_ptr + off_z);
        uint8_t intensity = off_intensity != -1 ? *reinterpret_cast<const uint8_t*>(point_ptr + off_intensity) : 255;
        
        if (remove_nan && (std::isnan(x) || std::isnan(y) || std::isnan(z))) {
            continue;
        }
        points.push_back(Point3D{x, y, z, intensity});
    }
    return points;
}

void save_xyz_to_pcd(const std::vector<Point3D>& points, const std::string& file_path){
    std::ofstream fs(file_path);
    if (!fs.is_open()) {
        std::cerr << "Error: cannot open file " << file_path << " for writing." << std::endl;
        return;
    }

    // PCD 0.7 Header
    fs << "# .PCD v0.7 - Point Cloud Data file format\n"
       << "VERSION 0.7\n"
       << "FIELDS x y z\n"
       << "SIZE 4 4 4\n"
       << "TYPE F F F\n"
       << "COUNT 1 1 1\n"
       << "WIDTH " << points.size() << "\n"
       << "HEIGHT 1\n"
       << "VIEWPOINT 0 0 0 1 0 0 0\n"
       << "POINTS " << points.size() << "\n"
       << "DATA ascii\n";
    
    for (const auto& p: points) {
        fs << p.x << " " << p.y << " " << p.z << "\n";
    }

    fs.close();
    std::cout << "Successfully saved " << points.size() << " points to " << file_path << std::endl;
}

std::vector<Point3D> depth_rgb_to_pointcloud(
    const std::shared_ptr<cv::Mat>& depth_img,
    const std::shared_ptr<cv::Mat>& rgb_img,
    float fx,
    float fy,
    float cx,
    float cy,
    float depth_scale = 0.1){
    
    std::vector<Point3D> points;
    std::cout << "depth_img->type: " << depth_img->type() << std::endl;

    if (depth_img->empty() || rgb_img->empty() || depth_img->rows != rgb_img->rows || depth_img->cols != rgb_img->cols) {
        std::cerr << "Error: depth image or rgb image is empty, or their sizes do not match." << std::endl;
        return points;
    } 
    std::cout << "depth_img->rows: " << depth_img->rows << std::endl;
    std::cout << "depth_img->cols: " << depth_img->cols << std::endl;
    std::cout << "fx: " << fx << std::endl;
    std::cout << "fy: " << fy << std::endl;
    std::cout << "cx: " << cx << std::endl;
    std::cout << "cy: " << cy << std::endl;
    std::cout << "depth_scale: " << depth_scale << std::endl;
    
    points.reserve(depth_img->rows * depth_img->cols);

    /** Handle different depth image types */
    int depth_type = depth_img->type();
    for (int v = 0; v < depth_img->rows; ++v) {
        for (int u = 0; u < depth_img->cols; ++u) {
            float depth = 0.0f;

            if (depth_type == CV_16UC1) {
                /** 16-bit unsigned single channel */
                uint16_t raw_depth = depth_img->at<uint16_t>(v, u);
                depth = static_cast<float>(raw_depth) * depth_scale;
            } else if (depth_type == CV_32F) {
                /** 32-bit float single channel */
                depth = depth_img->at<float>(v, u) * depth_scale;
            } else {
                std::cerr << "Error: unsupported depth image type: " << depth_type << std::endl;
                return points;
            }

            if (depth <= 0.0f) continue;
            float x = (u - cx) * depth / fx;
            float y = (v - cy) * depth / fy;
            uint8_t rgb = rgb_img->at<cv::Vec3b>(v, u)[0];

            points.push_back(Point3D{x, y, depth, rgb});
        }
    }
    return points;
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G1); 
        
        /** Get RGB and depth images from the left arm, depth images from the right arm,  
            base LiDAR data, and torso IMU data */
        std::unordered_set<SensorType> sensor_types = {
            SensorType::LEFT_ARM_CAMERA,        /** Left arm rgb camera */
            SensorType::LEFT_ARM_DEPTH_CAMERA,  /** Left arm depth camera */
            SensorType::BASE_LIDAR,              /** Base LiDAR */
            SensorType::TORSO_IMU              /** Torso IMU */
        };

        /* Initialize robot */
        if (robot.init(sensor_types)) {
            std::cout << "Initialization successful" << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        
        /** Get rgb image */
        std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::LEFT_ARM_CAMERA, RgbOutputFormat::JPEG, true);
        std::shared_ptr<cv::Mat> rgb_img;
        if (rgb_data) {
            std::cout << "Get rgb image suceess" << std::endl;
            rgb_img = rgb_data->convert_to_cv2_mat();
            
            cv::imwrite("rgb_image.png", *rgb_img);
            std::cout << "RGB image saved as 'rgb_image.png'" << std::endl;
        }else{
            std::cerr << "No rgb image data!" << std::endl;
        }

        /** Get depth image */
        std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
        std::shared_ptr<cv::Mat> depth_img;
        if (depth_data) {
            std::cout << "Get depth image suceess" << std::endl;
            depth_img = depth_data->convert_to_cv2_mat();

            cv::imwrite("depth_image.png", *depth_img);
            std::cout << "Depth image saved as 'depth_image.png'" << std::endl;
        }else{
            std::cerr << "No depth image data!" << std::endl;
        }

        /** Get lidar data */
        std::shared_ptr<LidarData> lidar_data = robot.get_lidar_data(SensorType::BASE_LIDAR);
        if (lidar_data) {
            std::vector<Point3D> points = get_xyz_points(lidar_data);
            save_xyz_to_pcd(points, "lidar_points.pcd");
        } else {
            std::cerr << "No lidar data!" << std::endl;
        }
        
        /** Get torso imu data */
        std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::TORSO_IMU);
        if (imu_data) {
            std::cout << "Get imu data suceess" << std::endl;
        } else {
            std::cerr << "No imu data!" << std::endl;
        }
        
        if (rgb_img && depth_img) {
            /** Set default camera intrinsics */
            std::shared_ptr<CameraInfo> camera_info = std::make_shared<CameraInfo>();
            camera_info->height = 720;
            camera_info->width = 1280;
            camera_info->k = {
                653.4349365234375, 0.0, 639.95159912109375, 
                0.0, 652.48858642578125, 365.29425048828125, 
                0.0, 0.0, 1.0
            };

            std::vector<Point3D> fusion_pcd = depth_rgb_to_pointcloud(
                depth_img,
                rgb_img,
                camera_info->k[0],
                camera_info->k[4],
                camera_info->k[2],
                camera_info->k[5],
                0.1);
            save_xyz_to_pcd(fusion_pcd, "fusion_pcd.pcd");
        }

        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    

    return 0;
}

示例 5：执行 VLA

适用场景：展示 VLA 客户端如何通过 get_synced_observation 采集同步观测、接收包含头部的 23 维绝对关节 action chunk，并使用高级实时关节控制接口 set_joint_commands 按控制频率逐帧快速执行。Galbot 官方支持通过主从臂采集示教数据，具体方法请参考 TM01 使用手册。采集生成的 MCAP 数据可通过 galbot-mcap2lerobot 转换为 LeRobot 数据集，用于后续模型训练。完成模型训练并部署推理服务后，可参考本示例完成同步观测获取和 action chunk 执行。若模型输出末端 EE pose，请参考 set_end_effector_command 接口。

examples/cpp/tutorials/src/example5_execute_vla.cpp

/**
 * Execute absolute-joint VLA action chunks with Galbot SDK interfaces.
 *
 * VLA control requires observations and actions to stay aligned in time. This
 * example uses get_synced_observation() to acquire a synchronized camera image
 * and robot joint state for each model request. It then uses
 * set_joint_commands() to stream the model's 23-dimensional absolute-joint
 * action chunk frame by frame at the configured control frequency.
 *
 * For the training-data workflow, Galbot officially supports demonstration
 * data collection with leader-follower arms. See the TM01 user manual:
 * https://developer.galbot.com/docs/tm01/1.4.0/zh/tm01
 * The collected MCAP data can be converted to the LeRobot dataset format with
 * galbot-mcap2lerobot for subsequent model training:
 * https://github.com/GalaxyGeneralRobotics/galbot-mcap2lerobot
 * After deploying the trained model as an inference service, use this example
 * as a reference for synchronized observation acquisition and action-chunk
 * execution on the robot.
 *
 * This example demonstrates absolute joint-position model output. If a model
 * instead outputs end-effector poses (EE poses such as
 * [x, y, z, qx, qy, qz, qw]), use set_end_effector_command() for real-time
 * Cartesian control. See
 * examples/g1/cpp/galbot_robot/src/set_end_effector_commands_example.cpp.
 *
 * Run: ./tutorials_example5_execute_vla <path/to/example5_vla.json>
 *
 * Before running, release the emergency-stop button and clear the area around
 * the robot. Keep a hand near the emergency-stop button during execution.
 */

#include <algorithm>
#include <chrono>
#include <cctype>
#include <cmath>
#include <cstddef>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <string>
#include <thread>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include "galbot_robot.hpp"

namespace {

using galbot::sdk::ControlStatus;
using galbot::sdk::GalbotRobot;
using galbot::sdk::JointCommand;
using galbot::sdk::SensorType;

constexpr double kControlFrequencyHz = 20.0;
// Number of model action frames returned and executed in one chunk.
constexpr std::size_t kActionChunkSize = 30;
constexpr double kGripperVelocity = 0.5;
constexpr double kGripperEffort = 10.0;
constexpr char kTaskPrompt[] =
    "Based on the current view, predict the next robot action.";

const std::vector<std::string> kActionGroups = {
    "leg",          "head",      "left_arm",
    "left_gripper", "right_arm", "right_gripper",
};

const std::unordered_map<std::string, std::pair<std::size_t, std::size_t>>
    kGroupSlices = {
        {"leg", {0, 5}},           {"head", {5, 7}},
        {"left_arm", {7, 14}},     {"left_gripper", {14, 15}},
        {"right_arm", {15, 22}},   {"right_gripper", {22, 23}},
};

const std::unordered_map<std::string, std::vector<std::string>>
    kGroupJointNames = {
        {"leg",
         {"leg_joint1", "leg_joint2", "leg_joint3", "leg_joint4",
          "leg_joint5"}},
        {"head", {"head_joint1", "head_joint2"}},
        {"left_arm",
         {"left_arm_joint1", "left_arm_joint2", "left_arm_joint3",
          "left_arm_joint4", "left_arm_joint5", "left_arm_joint6",
          "left_arm_joint7"}},
        {"left_gripper", {"left_gripper_joint1"}},
        {"right_arm",
         {"right_arm_joint1", "right_arm_joint2", "right_arm_joint3",
          "right_arm_joint4", "right_arm_joint5", "right_arm_joint6",
          "right_arm_joint7"}},
        {"right_gripper", {"right_gripper_joint1"}},
};

struct VlaObservation {
  std::vector<double> state;
  std::size_t image_bytes = 0;
  std::string prompt;
};

struct MockVlaResponse {
  std::vector<std::vector<double>> actions;
  std::size_t next_cursor = 0;
};

std::vector<std::string> action_joint_names() {
  std::vector<std::string> names;
  for (const auto& group : kActionGroups) {
    const auto& group_names = kGroupJointNames.at(group);
    names.insert(names.end(), group_names.begin(), group_names.end());
  }
  return names;
}

std::string action_json_path(int argc, char* argv[]) {
  if (argc > 1) {
    return argv[1];
  }
  return "examples/g1/assets/tutorials/example5_vla.json";
}

void confirm_safe_environment() {
  std::cout
      << "WARNING: Release the emergency-stop button, keep the robot in "
         "working mode, and clear people and obstacles from its motion range."
      << std::endl;
  std::cout << "Continue with the VLA execution example? [y/N]: ";

  std::string answer;
  std::cin >> answer;
  std::transform(answer.begin(), answer.end(), answer.begin(),
                 [](unsigned char character) {
                   return static_cast<char>(std::tolower(character));
                 });
  if (answer != "y" && answer != "yes") {
    throw std::runtime_error("Execution cancelled by the user");
  }
}

void validate_action_chunk(
    const std::vector<std::vector<double>>& actions) {
  constexpr std::size_t kExpectedDimension = 23;
  for (std::size_t frame_index = 0; frame_index < actions.size();
       ++frame_index) {
    if (actions[frame_index].size() != kExpectedDimension) {
      throw std::runtime_error(
          "VLA action frame " + std::to_string(frame_index) + " has " +
          std::to_string(actions[frame_index].size()) +
          " values; expected 23");
    }
    for (double value : actions[frame_index]) {
      if (!std::isfinite(value)) {
        throw std::runtime_error("VLA actions contain NaN or infinity");
      }
    }
  }
}

std::vector<std::vector<double>> load_mock_actions(
    const std::string& json_path) {
  std::ifstream file(json_path);
  if (!file.is_open()) {
    throw std::runtime_error("Mock VLA action file does not exist: " +
                             json_path);
  }

  boost::property_tree::ptree payload;
  boost::property_tree::read_json(file, payload);

  std::vector<std::vector<double>> actions;
  for (const auto& frame_node : payload.get_child("frames")) {
    std::vector<double> frame;
    for (const auto& value_node : frame_node.second) {
      frame.push_back(value_node.second.get_value<double>());
    }
    actions.push_back(std::move(frame));
  }
  const std::size_t declared_frames = payload.get<std::size_t>("num_frames");
  if (declared_frames != actions.size()) {
    throw std::runtime_error(
        "Mock num_frames does not match the number of JSON frames: " +
        std::to_string(declared_frames) + " != " +
        std::to_string(actions.size()));
  }
  validate_action_chunk(actions);
  return actions;
}

VlaObservation collect_observation(GalbotRobot& robot) {
  const std::vector<SensorType> cameras = {SensorType::HEAD_LEFT_CAMERA};
  auto observation = robot.get_synced_observation(cameras, true);
  if (!observation) {
    throw std::runtime_error("get_synced_observation failed");
  }

  const auto image_iterator =
      observation->rgb_data_map.find(SensorType::HEAD_LEFT_CAMERA);
  if (image_iterator == observation->rgb_data_map.end() ||
      !image_iterator->second) {
    throw std::runtime_error("Synchronized head-left image is missing");
  }
  if (!observation->joint_state) {
    throw std::runtime_error("Synchronized joint state is missing");
  }

  std::unordered_map<std::string, double> state_by_name;
  for (const auto& joint_state :
       observation->joint_state->joint_state_vec) {
    state_by_name[joint_state.joint_name] = joint_state.position;
  }

  VlaObservation result;
  for (const auto& joint_name : action_joint_names()) {
    const auto iterator = state_by_name.find(joint_name);
    if (iterator == state_by_name.end()) {
      throw std::runtime_error("Joint state is missing joint: " + joint_name);
    }
    result.state.push_back(iterator->second);
  }
  result.image_bytes = image_iterator->second->data.size();
  result.prompt = kTaskPrompt;
  return result;
}

MockVlaResponse mock_vla_server(
    const VlaObservation& observation,
    const std::vector<std::vector<double>>& all_actions, std::size_t cursor,
    std::size_t chunk_size = kActionChunkSize) {
  if (observation.state.size() != 23) {
    throw std::runtime_error("Observation state must contain 23 joints");
  }

  const std::size_t end =
      std::min(cursor + chunk_size, all_actions.size());
  MockVlaResponse response;
  response.actions.assign(all_actions.begin() + cursor,
                          all_actions.begin() + end);
  response.next_cursor = end;

  std::cout << "Mock VLA server received one request: prompt='"
            << observation.prompt << "', image_bytes="
            << observation.image_bytes << ", frames=" << cursor << ":" << end
            << std::endl;
  std::cout << "Mock VLA server returned action chunk: shape=("
            << response.actions.size() << ", 23)" << std::endl;
  return response;
}

std::vector<double> values_for_groups(
    const std::vector<double>& action,
    const std::vector<std::string>& groups) {
  std::vector<double> values;
  for (const auto& group : groups) {
    const auto [start, end] = kGroupSlices.at(group);
    values.insert(values.end(), action.begin() + start, action.begin() + end);
  }
  return values;
}

void move_to_first_action(GalbotRobot& robot,
                          const std::vector<double>& first_action) {
  const std::vector<std::vector<std::string>> movement_stages = {
      {"leg"},
      {"head", "left_arm", "right_arm"},
  };
  for (const auto& groups : movement_stages) {
    const ControlStatus status = robot.set_joint_positions(
        values_for_groups(first_action, groups), groups, {}, true, 0.2, 10.0);
    if (status != ControlStatus::SUCCESS) {
      throw std::runtime_error(
          "Failed to move a joint group stage to the first JSON action");
    }
  }

  const ControlStatus left_status = robot.set_gripper_command(
      "left_gripper", first_action[kGroupSlices.at("left_gripper").first], 0.1,
      kGripperEffort, true);
  if (left_status != ControlStatus::SUCCESS) {
    throw std::runtime_error(
        "Failed to move left_gripper to the first JSON action");
  }

  const ControlStatus right_status = robot.set_gripper_command(
      "right_gripper", first_action[kGroupSlices.at("right_gripper").first],
      0.1, kGripperEffort, true);
  if (right_status != ControlStatus::SUCCESS) {
    throw std::runtime_error(
        "Failed to move right_gripper to the first JSON action");
  }
  std::cout << "Robot reached the first pose in the VLA action file."
            << std::endl;
}

std::vector<JointCommand> build_joint_commands(
    const std::vector<double>& action) {
  if (action.size() != 23) {
    throw std::runtime_error("Action must contain 23 joint values");
  }

  std::vector<JointCommand> commands(action.size());
  for (std::size_t index = 0; index < action.size(); ++index) {
    commands[index].position = action[index];
    if (index == 14 || index == 22) {
      commands[index].velocity = kGripperVelocity;
      commands[index].effort = kGripperEffort;
    }
  }
  return commands;
}

void execute_action_chunk(
    GalbotRobot& robot,
    const std::vector<std::vector<double>>& actions) {
  validate_action_chunk(actions);
  if (actions.empty()) {
    std::cout << "The VLA model returned an empty chunk." << std::endl;
    return;
  }

  const auto period = std::chrono::duration_cast<std::chrono::steady_clock::duration>(
      std::chrono::duration<double>(1.0 / kControlFrequencyHz));
  auto next_tick = std::chrono::steady_clock::now();
  std::cout << "Streaming " << actions.size() << " frames at "
            << kControlFrequencyHz << " Hz with set_joint_commands..."
            << std::endl;

  for (std::size_t frame_index = 0; frame_index < actions.size();
       ++frame_index) {
    const ControlStatus status = robot.set_joint_commands(
        build_joint_commands(actions[frame_index]), kActionGroups, {}, 0.0);
    if (status != ControlStatus::SUCCESS) {
      throw std::runtime_error("set_joint_commands failed at frame " +
                               std::to_string(frame_index + 1));
    }
    next_tick += period;
    std::this_thread::sleep_until(next_tick);
  }
  std::cout << "Action chunk execution completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "SDK resources released." << std::endl;
}

}  // namespace

int main(int argc, char* argv[]) {
  using galbot::sdk::MachineType;

  GalbotRobot& robot = GalbotRobot::get_instance(MachineType::G1);
  bool initialized = false;
  try {
    confirm_safe_environment();
    const std::unordered_set<SensorType> enabled_sensors = {
        SensorType::HEAD_LEFT_CAMERA};
    if (!robot.init(enabled_sensors, true)) {
      throw std::runtime_error("GalbotRobot initialization failed");
    }
    initialized = true;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    const std::string json_path = action_json_path(argc, argv);
    const auto all_actions = load_mock_actions(json_path);
    std::cout << "Loaded VLA actions from: " << json_path << std::endl;
    std::cout << "Action data shape: (" << all_actions.size()
              << ", 23), including head" << std::endl;
    move_to_first_action(robot, all_actions.front());

    std::size_t cursor = 0;
    while (true) {
      const VlaObservation observation = collect_observation(robot);
      MockVlaResponse response =
          mock_vla_server(observation, all_actions, cursor);
      cursor = response.next_cursor;
      if (response.actions.empty()) {
        std::cout << "Mock VLA server has no more actions." << std::endl;
        break;
      }
      execute_action_chunk(robot, response.actions);
    }

    std::cout << "VLA execution example finished successfully." << std::endl;
    shutdown_robot(robot);
    initialized = false;
  } catch (const std::exception& error) {
    std::cerr << "Error: " << error.what() << std::endl;
    if (initialized) {
      shutdown_robot(robot);
    }
    return 1;
  }
  return 0;
}

示例 6：综合任务：导航+感知+抓取+放置

适用场景：完整抓取放置应用的参考示例，串联感知、运动规划与控制和导航，展示预抓取、抬升、回撤、抓取与放置之间的导航，以及使用 move_line 完成直线抓取和放置。

examples/cpp/tutorials/src/example6_pick_and_place.cpp

// This tutorial demonstrates a complete pick-and-place task for a standard application scenario.
// It integrates navigation, perception, motion planning, and robot control, and can be used as a
// reference implementation for standard applications.
// Before the task starts, the robot moves to the known 21-joint initial pose defined by this example.
// After picking, the robot navigates to its current map pose with the X coordinate offset by 0.1 m,
// then performs placement.
// The final grasp approach and place descent use move_line; other end-effector motions use
// set_end_effector_pose.

#include <array>
#include <chrono>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_set>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <opencv2/opencv.hpp>

using namespace galbot::sdk;

constexpr double kPlaceNavigationXOffsetM = 0.1;
constexpr double kGraspTransitionOffsetM = 0.05;
const std::vector<double> kPlacePoseBase = {0.4, 0.3, 0.7, 0.0, 0.0, 0.0, 1.0};
// Placeholder grasp pose for this runnable tutorial. Replace it with the
// base-frame pose produced by the application's perception module.
const std::vector<double> kGraspPoseBasePlaceholder = {0.4, 0.3, 0.65, 0.0, 0.0, 0.0, 1.0};
const std::vector<std::string> kInitialLegJointNames = {
    "leg_joint1", "leg_joint2", "leg_joint3", "leg_joint4", "leg_joint5",
};
const std::vector<double> kInitialLegJointPositions = {0.4, 1.2, 0.8, 0.0, 0.0};

const std::vector<std::string> kInitialUpperBodyJointNames = {
    "head_joint1",      "head_joint2",      "left_arm_joint1",  "left_arm_joint2",  "left_arm_joint3",
    "left_arm_joint4",  "left_arm_joint5",  "left_arm_joint6",  "left_arm_joint7",  "right_arm_joint1",
    "right_arm_joint2", "right_arm_joint3", "right_arm_joint4", "right_arm_joint5", "right_arm_joint6",
    "right_arm_joint7",
};
const std::vector<double> kInitialUpperBodyJointPositions = {
    0.0, 0.0, 1.90, -1.46, -0.54, -1.96, 0.0, -0.4, 0.0, -1.90, 1.46, 0.54, 1.96, 0.0, 0.4, 0.0,
};

bool move_to_initial_pose(GalbotRobot& robot) {
  const ControlStatus leg_status =
      robot.set_joint_positions(kInitialLegJointPositions, {}, kInitialLegJointNames, true, 0.2, 20.0);
  if (leg_status != ControlStatus::SUCCESS) {
    std::cout << "❌ Failed to move the legs to the initial pose: status=" << (int) leg_status << std::endl;
    return false;
  }
  std::cout << "✅ Successfully moved the legs to the initial pose: status=" << (int) leg_status << std::endl;

  const ControlStatus upper_body_status = robot.set_joint_positions(
      kInitialUpperBodyJointPositions, {}, kInitialUpperBodyJointNames, true, 0.2, 20.0);
  if (upper_body_status != ControlStatus::SUCCESS) {
    std::cout << "❌ Failed to move the head and arms to the initial pose: status=" << (int) upper_body_status
              << std::endl;
    return false;
  }

  std::cout << "✅ Successfully moved the head and arms to the initial pose: status=" << (int) upper_body_status
            << std::endl;
  return true;
}

static std::vector<double> pose_to_vector7(const Pose& p) {
  return {p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w};
}

// Quaternion and rotation matrix utilities (without Eigen)
struct QuaternionST {
  double x, y, z, w;

  QuaternionST(double x = 0, double y = 0, double z = 0, double w = 1) : x(x), y(y), z(z), w(w) {}

  // Convert quaternion to 3x3 rotation matrix
  std::array<std::array<double, 3>, 3> to_matrix() const {
    std::array<std::array<double, 3>, 3> mat;

    double xx = x * x, yy = y * y, zz = z * z;
    double xy = x * y, xz = x * z, yz = y * z;
    double wx = w * x, wy = w * y, wz = w * z;

    mat[0][0] = 1 - 2 * (yy + zz);
    mat[0][1] = 2 * (xy - wz);
    mat[0][2] = 2 * (xz + wy);

    mat[1][0] = 2 * (xy + wz);
    mat[1][1] = 1 - 2 * (xx + zz);
    mat[1][2] = 2 * (yz - wx);

    mat[2][0] = 2 * (xz - wy);
    mat[2][1] = 2 * (yz + wx);
    mat[2][2] = 1 - 2 * (xx + yy);

    return mat;
  }

};

// 4x4 transformation matrix
struct Transform {
  std::array<std::array<double, 4>, 4> mat;

  Transform() {
    // Identity matrix
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        mat[i][j] = (i == j) ? 1.0 : 0.0;
      }
    }
  }

  // Set rotation from quaternion
  void set_rotation(const QuaternionST& q) {
    auto rot = q.to_matrix();
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        mat[i][j] = rot[i][j];
      }
    }
  }

  // Set translation
  void set_translation(const std::array<double, 3>& t) {
    mat[0][3] = t[0];
    mat[1][3] = t[1];
    mat[2][3] = t[2];
  }

  // Transform a 3D point
  std::array<double, 3> transform_point(const std::array<double, 3>& p) const {
    std::array<double, 3> result;
    for (int i = 0; i < 3; ++i) {
      result[i] = mat[i][0] * p[0] + mat[i][1] * p[1] + mat[i][2] * p[2] + mat[i][3];
    }
    return result;
  }

};

void navigation_to_goal(GalbotNavigation& nav, const std::vector<double>& goal_pose, int retry_cnt = 3) {
  try {
    auto cur_pose = nav.get_current_pose();
    std::cout << "Current pose: [";
    for (auto val : pose_to_vector7(cur_pose))
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    Pose goal(goal_pose);
    if (nav.check_path_reachability(goal, cur_pose)) {
      retry_cnt = 3;
      NavigationStatus status;

      while (true) {
        status = nav.navigate_to_goal(goal, true, true, 20.0f);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        retry_cnt--;

        if (nav.check_goal_arrival() || retry_cnt < 0) {
          break;
        } else {
          std::cout << "Navigation failed: status=" << (int) status << ", retrying: " << retry_cnt << std::endl;
        }
      }

      std::cout << "navigate_to_goal return status: " << (int) status << std::endl;
      std::cout << "Has arrived: " << nav.check_goal_arrival() << std::endl;
    } else {
      std::cout << "Path unreachable or unsafe" << std::endl;
    }
  } catch (const std::exception& e) {
    std::cout << "Exception occurred during navigation: " << e.what() << std::endl;
  }
}

std::vector<double> pose_camera_to_base(GalbotRobot& robot, const std::vector<double>& pose_camera) {
  std::string source_frame = "left_arm_camera_color_optical_frame";
  std::string target_frame = "base_link";

  auto [base_to_cam, success] = robot.get_transform(target_frame, source_frame);

  if (!success || base_to_cam.empty()) {
    std::cout << "Failed to get transform from camera to chassis" << std::endl;
    return {};
  } else {
    std::cout << "base_to_cam: [";
    for (auto val : base_to_cam)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
  }

  Transform base_to_cam_mat;
  QuaternionST quat(base_to_cam[3], base_to_cam[4], base_to_cam[5], base_to_cam[6]);
  base_to_cam_mat.set_rotation(quat);
  base_to_cam_mat.set_translation({base_to_cam[0], base_to_cam[1], base_to_cam[2]});

  std::array<double, 3> cam_pos = {pose_camera[0], pose_camera[1], pose_camera[2]};
  auto pose_base = base_to_cam_mat.transform_point(cam_pos);

  return {pose_base[0], pose_base[1], pose_base[2], 0.0, 0.0, 0.0, 1.0};
}

std::vector<double> detect_object(GalbotRobot& robot) {
  const std::vector<double> k_zero_pose = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

  try {
    // This example uses the left-arm RGB and depth cameras.
    auto rgb_image_data = robot.get_rgb_data(SensorType::LEFT_ARM_CAMERA);
    auto depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);

    // Match Python: need both RGB and depth before running detection (otherwise NameError there).
    if (!rgb_image_data) {
      std::cout << "No rgb image data!" << std::endl;
      return k_zero_pose;
    }
    std::cout << "Get rgb image success" << std::endl;

    if (!depth_data) {
      std::cout << "No depth_data!" << std::endl;
      return k_zero_pose;
    }
    std::cout << "Get depth data success" << std::endl;

    auto img_ptr = rgb_image_data->convert_to_cv2_mat();
    auto depth_ptr = depth_data->convert_to_cv2_mat();
    if (!img_ptr || img_ptr->empty()) {
      std::cout << "Failed to decode rgb image" << std::endl;
      return k_zero_pose;
    }
    if (!depth_ptr || depth_ptr->empty()) {
      std::cout << "Failed to decode depth image" << std::endl;
      return k_zero_pose;
    }

    cv::Mat img = *img_ptr;
    cv::Mat depth_img = *depth_ptr;

    // Placeholder perception result in the OpenCV camera frame (x right, y down, z forward).
    // Replace this pose with the output of an application-specific RGB-D detector.
    std::vector<double> object_pose_camera = {0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71};

    std::cout << "object_pose_camera: [";
    for (auto val : object_pose_camera)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    auto object_pose_base = pose_camera_to_base(robot, object_pose_camera);
    if (object_pose_base.size() != 7) {
      std::cout << "Target pose in chassis coordinate system: (invalid / transform failed)" << std::endl;
      return k_zero_pose;
    }
    std::cout << "Target pose in chassis coordinate system: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    return object_pose_base;

  } catch (const std::exception& e) {
    std::cout << "Target detection exception: " << e.what() << std::endl;
    return k_zero_pose;
  }
}

void check_robot_safety() {
  std::cout << "⚠️  Note: 1. Please ensure the emergency stop button of the robot is released; "
            << "2. Please ensure there are no obstructions around the robot to avoid unexpected situations. "
            << "3. Please ensure the area around the robot is clear of obstacles." << std::endl;

  while (true) {
    std::cout << "Please confirm that the robot's emergency stop button is released "
              << "and there are no obstructions, continue? (y/n)..." << std::endl;

    std::string key;
    std::cin >> key;

    if (key == "y" || key == "Y") {
      std::cout << "User confirmed, continuing..." << std::endl;
      break;
    } else if (key == "n" || key == "N") {
      std::cout << "User did not confirm, exiting program..." << std::endl;
      exit(1);
    } else {
      std::cout << "Invalid input, please enter 'y' or 'n'" << std::endl;
    }
  }
}

bool pick_and_place(GalbotRobot& robot, GalbotNavigation& nav, GalbotMotion& motion,
                    const std::vector<double>& object_pose_base, bool navigation_enabled) {
  try {
    // Attach tool to end effector
    auto status = motion.attach_tool("left_arm", "galbot_gripper");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to attach tool: status=" << (int) status << std::endl;
      return false;
    } else {
      std::cout << "✅ Successfully attached tool: status=" << (int) status << std::endl;
    }

    // The target poses represent the attached gripper TCP, not the bare arm flange.
    // Tool pose mode makes set_end_effector_pose control the attached tool TCP.
    auto motion_params = std::make_shared<Parameter>();
    motion_params->set_direct_execute(true);
    motion_params->set_blocking(true);
    motion_params->set_timeout(20.0);
    motion_params->set_tool_pose(true);
    motion_params->set_check_collision(true);
    motion_params->set_reference_frame("base_link");
    motion_params->set_actuate("with_chain_only");

    // Open left gripper
    auto gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, false);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully set left gripper width to 0.1m: status=" << (int) gripper_status << std::endl;

    // This fixed grasp pose is only a placeholder for the runnable tutorial.
    // A real application should use object_pose_base from perception instead.
    std::cout << "Perception pose: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    std::vector<double> grasp_pose = kGraspPoseBasePlaceholder;
    std::vector<double> pre_grasp_pose = grasp_pose;
    pre_grasp_pose[0] -= kGraspTransitionOffsetM;
    std::vector<double> lift_grasp_pose = grasp_pose;
    lift_grasp_pose[2] += kGraspTransitionOffsetM;
    std::vector<double> retreat_pose = lift_grasp_pose;
    retreat_pose[0] -= kGraspTransitionOffsetM;

    std::cout << "pre_grasp_pose: [";
    for (auto val : pre_grasp_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(pre_grasp_pose, "left_arm", "base_link", nullptr, true, true, 20.0,
                                          motion_params);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute pre_grasp pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed pre_grasp pose command" << std::endl;

    // Use move_line for the final 5 cm straight-line approach to the object.
    MotionPlanChainTarget grasp_target;
    grasp_target.chain_name = "left_arm";
    grasp_target.mode = MotionPlanTargetMode::kCartesian;
    grasp_target.cart.chain_name = "left_arm";
    grasp_target.cart.frame_id = "EndEffector";
    grasp_target.cart.reference_frame = "base_link";
    grasp_target.cart.pose = Pose(grasp_pose);

    std::cout << "grasp_pose: [";
    for (auto val : grasp_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    auto move_line_result = motion.move_line({{grasp_target}}, *motion_params);
    status = std::get<0>(move_line_result);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute grasp move_line: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed grasp move_line" << std::endl;

    // Close gripper to grasp object
    gripper_status = robot.set_gripper_command("left_gripper", 0.02, 0.05, 10, false);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully closed left gripper: status=" << (int) gripper_status << std::endl;

    std::cout << "lift_grasp_pose: [";
    for (auto val : lift_grasp_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(lift_grasp_pose, "left_arm", "base_link", nullptr, true, true, 20.0,
                                          motion_params);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute lift_grasp pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed lift_grasp pose command" << std::endl;

    std::cout << "retreat_pose: [";
    for (auto val : retreat_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(retreat_pose, "left_arm", "base_link", nullptr, true, true, 20.0,
                                          motion_params);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute retreat pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed retreat pose command" << std::endl;

    // Return to the initial left-arm joint pose before navigation.
    const std::vector<std::string> left_arm_joint_names(kInitialUpperBodyJointNames.begin() + 2,
                                                        kInitialUpperBodyJointNames.begin() + 9);
    const std::vector<double> left_arm_joint_positions(kInitialUpperBodyJointPositions.begin() + 2,
                                                       kInitialUpperBodyJointPositions.begin() + 9);
    const ControlStatus left_arm_status =
        robot.set_joint_positions(left_arm_joint_positions, {}, left_arm_joint_names, true, 0.2, 20.0);
    if (left_arm_status != ControlStatus::SUCCESS) {
      std::cout << "❌ Failed to restore the left-arm initial pose: status=" << (int) left_arm_status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully restored the left-arm initial pose: status=" << (int) left_arm_status << std::endl;

    if (navigation_enabled) {
      auto place_navigation_goal = pose_to_vector7(nav.get_current_pose());
      place_navigation_goal[0] += kPlaceNavigationXOffsetM;
      std::cout << "Place navigation target pose: [";
      for (auto val : place_navigation_goal)
        std::cout << val << " ";
      std::cout << "]" << std::endl;
      navigation_to_goal(nav, place_navigation_goal);
      std::this_thread::sleep_for(std::chrono::seconds(2));
      std::cout << "✅ Navigation stage completed; starting Place" << std::endl;
    } else {
      std::cout << "⚠️ Navigation is unavailable; skipping navigation and continuing with Place at the current "
                   "position."
                << std::endl;
    }

    // Move above the place point, then descend 5 cm in a straight line.
    std::vector<double> place_pose = kPlacePoseBase;
    std::vector<double> pre_place_pose = place_pose;
    pre_place_pose[2] += kGraspTransitionOffsetM;
    std::cout << "pre_place_pose: [";
    for (auto val : pre_place_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(pre_place_pose, "left_arm", "base_link", nullptr, true, true, 20.0);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute pre_place pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed pre_place pose command" << std::endl;

    MotionPlanChainTarget place_target;
    place_target.chain_name = "left_arm";
    place_target.mode = MotionPlanTargetMode::kCartesian;
    place_target.cart.chain_name = "left_arm";
    place_target.cart.frame_id = "EndEffector";
    place_target.cart.reference_frame = "base_link";
    place_target.cart.pose = Pose(place_pose);

    std::cout << "place_pose: [";
    for (auto val : place_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    move_line_result = motion.move_line({{place_target}}, *motion_params);
    status = std::get<0>(move_line_result);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute place move_line: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed place move_line" << std::endl;

    // Release target
    gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, false);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully released left gripper: status=" << (int) gripper_status << std::endl;

    // Return the left arm to its initial joint pose after placing the object.
    const ControlStatus post_place_arm_status =
        robot.set_joint_positions(left_arm_joint_positions, {}, left_arm_joint_names, true, 0.2, 20.0);
    if (post_place_arm_status != ControlStatus::SUCCESS) {
      std::cout << "❌ Failed to restore the left-arm pose after placing: status=" << (int) post_place_arm_status
                << std::endl;
      return false;
    }
    std::cout << "✅ Successfully restored the left-arm pose after placing: status=" << (int) post_place_arm_status
              << std::endl;

    // Detach tool from end effector
    status = motion.detach_tool("left_arm");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to detach tool: status=" << (int) status << std::endl;
    } else {
      std::cout << "✅ Successfully detached tool: status=" << (int) status << std::endl;
    }

    return true;

  } catch (const std::exception& e) {
    std::cout << "Exception occurred during pick_and_place: " << e.what() << std::endl;
    return false;
  }
}

int main() {
  check_robot_safety();

  // Get robot instances
  auto& robot = GalbotRobot::get_instance(MachineType::G1);
  auto& motion = GalbotMotion::get_instance(MachineType::G1);
  auto& nav = GalbotNavigation::get_instance(MachineType::G1);

  try {
    // Enable sensors
    std::unordered_set<SensorType> enable_sensor_set = {SensorType::LEFT_ARM_CAMERA,
                                                        SensorType::LEFT_ARM_DEPTH_CAMERA};

    // Initialize robot
    if (robot.init(enable_sensor_set)) {
      std::cout << "GalbotRobot initialization successful" << std::endl;
    } else {
      std::cout << "GalbotRobot initialization failed" << std::endl;
    }

    if (motion.init()) {
      std::cout << "GalbotMotion initialization successful" << std::endl;
    } else {
      std::cout << "GalbotMotion initialization failed" << std::endl;
    }

    if (nav.init()) {
      std::cout << "GalbotNavigation initialization successful" << std::endl;
    } else {
      std::cout << "GalbotNavigation initialization failed" << std::endl;
    }

    // Wait for data readiness
    std::this_thread::sleep_for(std::chrono::seconds(1));

    if (!move_to_initial_pose(robot)) {
      throw std::runtime_error("Failed to move to the task initial pose");
    }

    // Check localization once. If unavailable, Pick still runs and navigation
    // between Pick and Place is skipped.
    bool navigation_enabled = false;
    try {
      navigation_enabled = nav.is_localized();
    } catch (const std::exception& e) {
      std::cout << "⚠️ Navigation status check failed: " << e.what() << std::endl;
    }
    if (navigation_enabled) {
      std::cout << "✅ Robot is localized; Place navigation is enabled." << std::endl;
    } else {
      std::cout << "⚠️ Robot is not localized; navigation will be skipped. This run demonstrates Pick and Place "
                   "at the current position."
                << std::endl;
    }

    std::cout << std::endl;

    // Detect the target before Pick.
    auto object_pose_base = detect_object(robot);
    std::cout << std::endl;

    if (object_pose_base.size() != 7) {
      std::cout << "Skipping pick_and_place: invalid object pose (expected 7 values)" << std::endl;
    } else {
      // Execute Pick, navigate, and then Place.
      if (!pick_and_place(robot, nav, motion, object_pose_base, navigation_enabled)) {
        throw std::runtime_error("Pick-and-place execution failed");
      }
    }
    std::cout << std::endl;

  } catch (const std::exception& e) {
    std::cout << "Exception occurred: " << e.what() << std::endl;
  }

  // Cleanup
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resource release successful" << std::endl;

  return 0;
}