C++ 示例

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

机器人关节名称（S1 1.2）

关节组列表

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

各关节组详细信息

关节组名称	关节组英文名	关节数量	关节名称列表
头部	head	2	head_joint1, head_joint2
躯干	torso	1	S1 V1：torso_base_joint；S1 V2：torso_lift_joint1
左臂	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

如何正确使用 joint_groups

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

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

传参规则：

1. 需要控制整条链路时，优先使用 joint_groups（torso/head/arm/gripper）。
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)，避免手写字符串出错。

S1 关节组典型场景

关节组	典型场景
torso	躯干高度/俯仰调整
head	头部朝向/相机对准
left_arm / right_arm	机械臂抓取与操作
left_gripper / right_gripper	夹爪开合宽度控制

说明：S1 还定义了底盘和相机挂载等被动关节组（如 swerve_chassis、left_camera、right_camera），通常不作为 set_joint_positions 的直接目标。

传感器类型与 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
HEAD_LIDAR	head_lidar_base_link
BACK_LIDAR	back_lidar_base_link
CHASSIS_LIDAR	chassis_lidar_base_link
HEAD_IMU	head_imu_base_link
BACK_IMU	back_imu_base_link
CHASSIS_IMU	chassis_imu_base_link
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

注意：调用 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::S1);
    
    // 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::S1);
    
    // 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::S1);
  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::S1);

    // 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 ["torso", "head", "left_arm", "right_arm"] (S1: torso replaces G1 leg)
    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 empty array defaults to torso, head, left_arm, right_arm (S1: torso replaces G1 leg)
    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
    // Head angles: within S1 limits [-0.7854, 0.7854] and [-0.6109, 0.6109]
    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::S1);

    // 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 = false;
    // 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;
}

S1 末端工具原始透传

以下示例展示如何编码、发送、校验和解析设备特定的 64 字节 CAN/RS485 帧。使用前请先阅读 S1 末端工具原始透传。

警告

WBCS 必须在 [robot_info.custom_params] 下设置 endtool_raw_enabled=true。不要让 Raw writer 与标准控制器或另一个写入相同 TIB 的客户端并发运行。

自研 CAN 夹爪

初始化并运动自研二指夹爪，然后从 250 Hz 接收流监控设备被动上报的状态。

examples/s1/cpp/galbot_robot/src/endtool_raw_grip_can_example.cpp

#include <array>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <cstdint>
#include <cstring>
#include <iomanip>
#include <iostream>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr uint32_t kTxCanId = 0x70A;
constexpr uint32_t kRxCanId = 0x10A;
constexpr uint8_t kWriteCommand = 0x21;
constexpr uint8_t kAnswerCommand = 0x22;
constexpr uint8_t kReportCommand = 0x23;
constexpr uint8_t kInitFunction = 0x04;
constexpr uint8_t kMoveFunction = 0x05;
constexpr float kMinWidthMm = 6.0F;
constexpr float kMaxWidthMm = 120.0F;
constexpr float kMinVelocityMmS = 1.0F;
constexpr float kMaxVelocityMmS = 100.0F;
constexpr float kMinTorqueNm = 1.0F;
constexpr float kMaxTorqueNm = 50.0F;
constexpr auto kStatusPollInterval = std::chrono::milliseconds(100);
constexpr auto kStatusStaleInterval = std::chrono::seconds(1);
constexpr auto kInitialStatusWait = std::chrono::seconds(1);
constexpr auto kStatusPrintInterval = std::chrono::milliseconds(500);
constexpr auto kRawTransportReadyTimeout = std::chrono::seconds(2);
constexpr auto kRawTransportWarmupInterval = std::chrono::milliseconds(200);
constexpr auto kInitTimeout = std::chrono::seconds(20);
constexpr auto kInitCommandGuardInterval = std::chrono::seconds(1);
constexpr auto kMonitorDuration = std::chrono::seconds(10);
constexpr float kPositionToleranceMm = 2.0F;
constexpr float kStoppedVelocityToleranceMmS = 1.0F;

enum class DecodeResult {
  VALID,
  IDLE_SNAPSHOT,
  FRAME_TYPE,
  CAN_ID,
  COMMAND,
  PAYLOAD,
};

struct GripStatus {
  float position_mm = 0.0F;
  float velocity_mm_s = 0.0F;
  float torque_nm = 0.0F;
  uint8_t motion_state = 0;
  uint8_t error_code = 0;
  uint8_t response_command = 0;
};

struct DemoState {
  std::mutex mutex;
  std::condition_variable changed;
  GripStatus last_status;
  bool valid = false;
  uint64_t last_generation = 0;
  uint64_t last_update_ms = 0;
  uint64_t latest_raw_generation = 0;
  uint64_t idle_snapshot_count = 0;
  uint64_t frame_type_reject_count = 0;
  uint64_t can_id_reject_count = 0;
  uint64_t command_reject_count = 0;
  uint64_t payload_reject_count = 0;
  uint64_t non_increasing_generation_count = 0;
};

struct StateSnapshot {
  GripStatus status;
  bool valid = false;
  uint64_t last_generation = 0;
  uint64_t last_update_ms = 0;
  uint64_t latest_raw_generation = 0;
  uint64_t idle_snapshot_count = 0;
  uint64_t frame_type_reject_count = 0;
  uint64_t can_id_reject_count = 0;
  uint64_t command_reject_count = 0;
  uint64_t payload_reject_count = 0;
  uint64_t non_increasing_generation_count = 0;
};

uint64_t steady_now_ms() {
  return static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                   std::chrono::steady_clock::now().time_since_epoch())
                                   .count());
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> make_can_frame(
    uint8_t command, uint8_t function, const uint8_t* payload, std::size_t payload_size) {
  std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> frame{};
  const uint32_t frame_type = 1;
  std::memcpy(frame.data(), &frame_type, sizeof(frame_type));
  std::memcpy(frame.data() + 4, &kTxCanId, sizeof(kTxCanId));
  frame[8] = static_cast<uint8_t>(payload_size + 2);
  frame[12] = command;
  frame[13] = function;
  if (payload != nullptr && payload_size > 0) {
    std::memcpy(frame.data() + 14, payload, payload_size);
  }
  return frame;
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_init() {
  return make_can_frame(kWriteCommand, kInitFunction, nullptr, 0);
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_move(
    float width_mm, float velocity_mm_s, float torque_nm) {
  std::array<uint8_t, 12> payload{};
  std::memcpy(payload.data(), &width_mm, sizeof(width_mm));
  std::memcpy(payload.data() + 4, &velocity_mm_s, sizeof(velocity_mm_s));
  std::memcpy(payload.data() + 8, &torque_nm, sizeof(torque_nm));
  return make_can_frame(kWriteCommand, kMoveFunction, payload.data(), payload.size());
}

DecodeResult decode_status(const EndToolRawData& raw, EndToolSide expected_side, GripStatus& status) {
  // This gripper's CAN response is valid only in the 250 Hz passthrough buffer.
  if (raw.side != expected_side || raw.kind != EndToolRxKind::BUFFER_250HZ) {
    return DecodeResult::FRAME_TYPE;
  }

  uint32_t frame_type = 0;
  uint32_t can_id = 0;
  std::memcpy(&frame_type, raw.frame.data(), sizeof(frame_type));
  std::memcpy(&can_id, raw.frame.data() + 4, sizeof(can_id));
  if (frame_type != 1) {
    return DecodeResult::FRAME_TYPE;
  }
  // A zero CAN ID is an empty slot in the 250 Hz buffer, not a device error.
  if (can_id == 0) {
    return DecodeResult::IDLE_SNAPSHOT;
  }
  if (can_id != kRxCanId) {
    return DecodeResult::CAN_ID;
  }

  // Valid S1 EtherCAT/TIB snapshots can have byte 12 cleared. Identify the
  // device report by its CAN ID and by the command/function fields at byte 16.
  const uint8_t command = raw.frame[16];
  const uint8_t function = raw.frame[17];
  if ((command != kAnswerCommand && command != kReportCommand) || function != kMoveFunction) {
    return DecodeResult::COMMAND;
  }

  GripStatus decoded;
  decoded.error_code = raw.frame[21];
  decoded.response_command = command;
  std::memcpy(&decoded.position_mm, raw.frame.data() + 22, sizeof(decoded.position_mm));
  std::memcpy(&decoded.velocity_mm_s, raw.frame.data() + 26, sizeof(decoded.velocity_mm_s));
  std::memcpy(&decoded.torque_nm, raw.frame.data() + 30, sizeof(decoded.torque_nm));
  decoded.motion_state = raw.frame[34];
  if (!std::isfinite(decoded.position_mm) || !std::isfinite(decoded.velocity_mm_s) ||
      !std::isfinite(decoded.torque_nm) || decoded.position_mm < 0.0F ||
      decoded.position_mm > kMaxWidthMm ||
      std::fabs(decoded.velocity_mm_s) > kMaxVelocityMmS ||
      std::fabs(decoded.torque_nm) > kMaxTorqueNm || decoded.motion_state > 4) {
    // An uninitialized gripper may report 0 mm even though Move accepts 6 mm
    // as its minimum target, so only negative positions are rejected here.
    return DecodeResult::PAYLOAD;
  }

  status = decoded;
  return DecodeResult::VALID;
}

const char* error_text(uint8_t error_code) {
  switch (error_code) {
    case 0x00:
      return "no error";
    case 0x01:
      return "not enabled";
    case 0x08:
      return "overvoltage";
    case 0x09:
      return "undervoltage";
    case 0x0A:
      return "overcurrent";
    case 0x0B:
      return "MOS overheating";
    case 0x0C:
      return "motor coil overheating";
    case 0x0D:
      return "communication lost";
    case 0x0E:
      return "motor overload";
    case 0x10:
      return "locked rotor";
    case 0x11:
      return "locked rotor and not enabled";
    case 0x40:
      return "power loss after calibration";
    case 0x80:
      return "communication error";
    default:
      return "unknown error";
  }
}

StateSnapshot get_snapshot(const std::shared_ptr<DemoState>& state) {
  std::lock_guard<std::mutex> lock(state->mutex);
  StateSnapshot snapshot;
  snapshot.status = state->last_status;
  snapshot.valid = state->valid;
  snapshot.last_generation = state->last_generation;
  snapshot.last_update_ms = state->last_update_ms;
  snapshot.latest_raw_generation = state->latest_raw_generation;
  snapshot.idle_snapshot_count = state->idle_snapshot_count;
  snapshot.frame_type_reject_count = state->frame_type_reject_count;
  snapshot.can_id_reject_count = state->can_id_reject_count;
  snapshot.command_reject_count = state->command_reject_count;
  snapshot.payload_reject_count = state->payload_reject_count;
  snapshot.non_increasing_generation_count = state->non_increasing_generation_count;
  return snapshot;
}

void count_decode_result(DemoState& state, DecodeResult result) {
  switch (result) {
    case DecodeResult::IDLE_SNAPSHOT:
      ++state.idle_snapshot_count;
      break;
    case DecodeResult::FRAME_TYPE:
      ++state.frame_type_reject_count;
      break;
    case DecodeResult::CAN_ID:
      ++state.can_id_reject_count;
      break;
    case DecodeResult::COMMAND:
      ++state.command_reject_count;
      break;
    case DecodeResult::PAYLOAD:
      ++state.payload_reject_count;
      break;
    case DecodeResult::VALID:
      break;
  }
}

void print_status(const char* prefix, uint64_t generation, const GripStatus& status) {
  std::cout << prefix << " generation=" << generation << " source="
            << (status.response_command == kAnswerCommand ? "ANSWER" : "REPORT")
            << " position=" << status.position_mm << "mm velocity=" << status.velocity_mm_s
            << "mm/s torque=" << status.torque_nm
            << "Nm motion_state=" << static_cast<unsigned int>(status.motion_state)
            << " error=0x" << std::hex << static_cast<unsigned int>(status.error_code) << std::dec
            << " (" << error_text(status.error_code) << ")\n";
}

void print_rx_summary(const StateSnapshot& snapshot) {
  std::cout << "RX summary: latest_raw_generation=" << snapshot.latest_raw_generation
            << " latest_valid_generation=" << snapshot.last_generation
            << " idle_snapshots=" << snapshot.idle_snapshot_count
            << " malformed={frame_type:" << snapshot.frame_type_reject_count
            << ",can_id:" << snapshot.can_id_reject_count
            << ",command:" << snapshot.command_reject_count
            << ",payload:" << snapshot.payload_reject_count << "}"
            << " non_increasing_generations=" << snapshot.non_increasing_generation_count
            << '\n';
}

bool parse_side(const std::string& value, EndToolSide& side) {
  if (value == "left" || value == "l" || value == "0") {
    side = EndToolSide::LEFT;
    return true;
  }
  if (value == "right" || value == "r" || value == "1") {
    side = EndToolSide::RIGHT;
    return true;
  }
  return false;
}

bool valid_motion_command(float width_mm, float velocity_mm_s, float torque_nm) {
  return std::isfinite(width_mm) && std::isfinite(velocity_mm_s) && std::isfinite(torque_nm) &&
         width_mm >= kMinWidthMm && width_mm <= kMaxWidthMm &&
         velocity_mm_s >= kMinVelocityMmS && velocity_mm_s <= kMaxVelocityMmS &&
         torque_nm >= kMinTorqueNm && torque_nm <= kMaxTorqueNm;
}

void print_usage(const char* program, std::ostream& output) {
  output << "Usage:\n"
         << "  " << program << " [options]\n"
         << "  " << program
         << " [left|right] [width_mm] [velocity_mm_s] [torque_nm] [init:0|1]"
            " [stop_controller:0|1] [restore_controller:0|1]\n\n"
         << "Options:\n"
         << "  -h, --help                    Show this help message.\n"
         << "  --side <left|right>           Gripper side (default: left).\n"
         << "  --width-mm <6..120>           Target opening in millimeters (default: 50).\n"
         << "  --velocity-mm-s <1..100>      Target velocity in mm/s (default: 50).\n"
         << "  --torque-nm <1..50>           Target torque in Nm (default: 20).\n"
         << "  --init                         Home/calibrate before Move.\n"
         << "  --stop-standard-controller    Best-effort stop of the standard controller.\n"
         << "  --restore-controller          Restart the standard controller on exit.\n\n"
         << "Examples:\n"
         << "  " << program << " --help\n"
         << "  " << program << " --side left --init\n"
         << "  " << program
         << " --side right --width-mm 50 --velocity-mm-s 50 --torque-nm 20 --init\n";
}

}  // namespace

int main(int argc, char** argv) {
  EndToolSide side = EndToolSide::LEFT;
  float width_mm = 50.0F;
  float velocity_mm_s = 50.0F;
  float torque_nm = 20.0F;
  int init_value = 0;
  int stop_controller_value = 0;
  int restore_controller_value = 0;
  try {
    // Named options match the Python example, while the original positional
    // form remains available for existing scripts.
    const bool named_options = argc > 1 && argv[1][0] == '-';
    if (named_options) {
      for (int index = 1; index < argc; ++index) {
        const std::string option = argv[index];
        if (option == "-h" || option == "--help") {
          print_usage(argv[0], std::cout);
          return 0;
        }

        const auto read_value = [&index, argc, argv](const std::string& name) {
          if (index + 1 >= argc) {
            throw std::invalid_argument("missing value for " + name);
          }
          return std::string(argv[++index]);
        };
        if (option == "--side") {
          const std::string value = read_value(option);
          if (!parse_side(value, side)) {
            throw std::invalid_argument("--side must be left or right");
          }
        } else if (option == "--width-mm") {
          width_mm = std::stof(read_value(option));
        } else if (option == "--velocity-mm-s") {
          velocity_mm_s = std::stof(read_value(option));
        } else if (option == "--torque-nm") {
          torque_nm = std::stof(read_value(option));
        } else if (option == "--init") {
          init_value = 1;
        } else if (option == "--stop-standard-controller") {
          stop_controller_value = 1;
        } else if (option == "--restore-controller") {
          restore_controller_value = 1;
        } else {
          throw std::invalid_argument("unknown option: " + option);
        }
      }
    } else {
      if (!parse_side(argc > 1 ? argv[1] : "left", side)) {
        throw std::invalid_argument("side must be left/right/l/r/0/1");
      }
      width_mm = argc > 2 ? std::stof(argv[2]) : width_mm;
      velocity_mm_s = argc > 3 ? std::stof(argv[3]) : velocity_mm_s;
      torque_nm = argc > 4 ? std::stof(argv[4]) : torque_nm;
      init_value = argc > 5 ? std::stoi(argv[5]) : init_value;
      stop_controller_value = argc > 6 ? std::stoi(argv[6]) : stop_controller_value;
      restore_controller_value = argc > 7 ? std::stoi(argv[7]) : restore_controller_value;
    }
  } catch (const std::exception& error) {
    std::cerr << "Argument error: " << error.what() << "\n\n";
    print_usage(argv[0], std::cerr);
    return 1;
  }
  if (!valid_motion_command(width_mm, velocity_mm_s, torque_nm) ||
      (init_value != 0 && init_value != 1) ||
      (stop_controller_value != 0 && stop_controller_value != 1) ||
      (restore_controller_value != 0 && restore_controller_value != 1)) {
    std::cerr << "Argument error: motion values are outside the documented ranges.\n\n";
    print_usage(argv[0], std::cerr);
    return 1;
  }

  const std::string controller_group = side == EndToolSide::LEFT ? "left_gripper" : "right_gripper";
  // Print the resolved values before SDK initialization so command-line
  // mistakes remain visible even when the robot connection cannot be created.
  std::cout << "Parsed options: side=" << (side == EndToolSide::LEFT ? "left" : "right")
            << " width_mm=" << width_mm << " velocity_mm_s=" << velocity_mm_s
            << " torque_nm=" << torque_nm << " init=" << std::boolalpha
            << (init_value == 1) << " stop_standard_controller="
            << (stop_controller_value == 1) << " restore_controller="
            << (restore_controller_value == 1) << std::noboolalpha << '\n';

  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  if (!robot.init()) {
    std::cerr << "FAILED: failed to initialize the S1 SDK.\n";
    return 2;
  }

  std::cout << "WARNING: same-process raw TX is serialized, but there is no cross-client "
               "resource lease.\n";
  std::cout << "WBCS must run with endtool_raw_enabled=true in [robot_info.custom_params].\n";
  if (stop_controller_value == 1) {
    const auto stop_status = robot.stop_controller(controller_group);
    std::cout << "Best-effort stop of " << controller_group << ": "
              << static_cast<int>(stop_status) << '\n';
  }

  auto state = std::make_shared<DemoState>();
  const uint64_t callback_handle = robot.register_endtool_raw_callback(
      [side, state](const EndToolRawData& raw) {
        if (raw.side != side || raw.kind != EndToolRxKind::BUFFER_250HZ) {
          return;
        }

        GripStatus status;
        const DecodeResult decode_result = decode_status(raw, side, status);
        std::unique_lock<std::mutex> lock(state->mutex);
        state->latest_raw_generation = raw.generation;
        if (decode_result != DecodeResult::VALID) {
          count_decode_result(*state, decode_result);
          lock.unlock();
          state->changed.notify_all();
          return;
        }

        if (raw.generation != 0 && state->last_generation != 0 &&
            raw.generation <= state->last_generation) {
          ++state->non_increasing_generation_count;
          lock.unlock();
          state->changed.notify_all();
          return;
        }
        state->last_status = status;
        state->valid = true;
        state->last_generation = raw.generation;
        state->last_update_ms = steady_now_ms();
        lock.unlock();
        state->changed.notify_all();
      });
  if (callback_handle == 0) {
    std::cerr << "FAILED: failed to register the raw RX callback.\n";
    if (restore_controller_value == 1) {
      robot.start_controller(controller_group);
    }
    robot.destroy();
    return 3;
  }

  bool success = true;
  std::cout << "Waiting for the first valid Raw status before TX (timeout=2s)...\n";
  {
    std::unique_lock<std::mutex> lock(state->mutex);
    success = state->changed.wait_for(lock, kRawTransportReadyTimeout, [&state] {
      return state->valid;
    });
  }
  if (!success) {
    const StateSnapshot snapshot = get_snapshot(state);
    std::cerr << "FAILED: raw transport did not deliver a valid gripper status before TX; "
              << "latest_raw_generation=" << snapshot.latest_raw_generation
              << " idle_snapshots=" << snapshot.idle_snapshot_count << '\n';
    print_rx_summary(snapshot);
  } else {
    const StateSnapshot snapshot = get_snapshot(state);
    std::cout << "Raw transport ready: generation=" << snapshot.last_generation
              << "; warming up TX for 200ms...\n";
    // Receiving a valid sample proves that discovery is active. Give the
    // command writer a short additional window to match the WBCS subscriber
    // before publishing the first one-shot Init or Move frame.
    std::this_thread::sleep_for(kRawTransportWarmupInterval);
  }

  if (success && init_value == 1) {
    std::cout << "WARNING: Init performs homing/calibration; do not run it before every Move.\n";
    const uint64_t init_baseline = get_snapshot(state).last_generation;
    if (robot.send_endtool_raw_frame(side, encode_init()) != ControlStatus::SUCCESS) {
      std::cerr << "FAILED: failed to publish the gripper init frame.\n";
      success = false;
    }

    if (success) {
      // A passive Ready report from the pre-Init state can arrive immediately
      // after publication. Keep Move out of the TX path for at least one second
      // so that report cannot prematurely complete this Init attempt.
      const uint64_t ready_not_before_ms =
          steady_now_ms() +
          static_cast<uint64_t>(
              std::chrono::duration_cast<std::chrono::milliseconds>(
                  kInitCommandGuardInterval)
                  .count());
      std::cout << "Waiting for a fresh Ready status (2) after Init "
                   "(minimum guard=1s, timeout=20s)...\n";
      const auto init_deadline = std::chrono::steady_clock::now() + kInitTimeout;
      auto last_print_at = std::chrono::steady_clock::time_point::min();
      uint8_t last_motion_state = 0;
      uint8_t last_error_code = 0;
      bool has_printed_state = false;
      bool ready = false;
      while (std::chrono::steady_clock::now() < init_deadline) {
        // Wake on a raw callback, but periodically re-check the timeout even if
        // the receive stream is silent.
        {
          std::unique_lock<std::mutex> lock(state->mutex);
          state->changed.wait_for(lock, kStatusPollInterval);
        }
        const StateSnapshot snapshot = get_snapshot(state);
        if (!snapshot.valid || snapshot.last_generation <= init_baseline) {
          continue;
        }

        const auto now = std::chrono::steady_clock::now();
        const bool state_changed = !has_printed_state ||
                                   snapshot.status.motion_state != last_motion_state ||
                                   snapshot.status.error_code != last_error_code;
        if (state_changed || now - last_print_at >= kStatusPrintInterval) {
          print_status("Init status:", snapshot.last_generation, snapshot.status);
          last_motion_state = snapshot.status.motion_state;
          last_error_code = snapshot.status.error_code;
          last_print_at = now;
          has_printed_state = true;
        }
        // State 2 is command-ready. State 4 can be a previous Move completion
        // and therefore does not prove that this Init has completed.
        if (snapshot.status.motion_state == 2 &&
            snapshot.last_update_ms >= ready_not_before_ms) {
          ready = true;
          break;
        }
      }
      if (!ready) {
        const StateSnapshot snapshot = get_snapshot(state);
        std::cerr << "FAILED: gripper did not report a fresh command-ready status after Init; "
                  << "latest_valid_generation=" << snapshot.last_generation
                  << " latest_raw_generation=" << snapshot.latest_raw_generation
                  << " idle_snapshots=" << snapshot.idle_snapshot_count << '\n';
        print_rx_summary(snapshot);
        success = false;
      }
    }
  }

  if (success) {
    // Give the passive stream a short chance to expose an uninitialized
    // device before publishing Move. No status is treated as inconclusive.
    {
      std::unique_lock<std::mutex> lock(state->mutex);
      state->changed.wait_for(lock, kInitialStatusWait, [&state] {
        return state->valid;
      });
      if (state->valid &&
          (state->last_status.error_code == 0x01 || state->last_status.motion_state == 0)) {
        std::cerr << "FAILED: gripper is not initialized (device error 0x01/state 0); "
                     "rerun this example with init=1 before sending Move.\n";
        success = false;
      }
    }
  }

  uint64_t move_baseline = 0;
  if (success) {
    move_baseline = get_snapshot(state).last_generation;
    if (robot.send_endtool_raw_frame(side, encode_move(width_mm, velocity_mm_s, torque_nm)) !=
        ControlStatus::SUCCESS) {
      std::cerr << "FAILED: failed to publish the gripper move frame.\n";
      success = false;
    }
  }

  if (success) {
    std::cout << "Move sent. Monitoring only fresh passive status reports for 10 seconds...\n";
    const auto deadline = std::chrono::steady_clock::now() + kMonitorDuration;
    auto last_sdk_print = std::chrono::steady_clock::time_point::min();
    auto last_raw_print = std::chrono::steady_clock::time_point::min();
    float last_sdk_position_mm = 0.0F;
    bool has_sdk_position = false;
    bool has_raw_state = false;
    uint8_t last_raw_motion_state = 0;
    uint8_t last_raw_error_code = 0;
    uint64_t printed_generation = move_baseline;
    bool received_after_move = false;
    bool raw_target_reached = false;
    bool sdk_target_reached = false;
    GripStatus latest_move_status;
    bool has_latest_move_status = false;

    while (std::chrono::steady_clock::now() < deadline) {
      {
        std::unique_lock<std::mutex> lock(state->mutex);
        state->changed.wait_for(lock, kStatusPollInterval);
      }
      const StateSnapshot snapshot = get_snapshot(state);

      if (snapshot.valid && snapshot.last_generation > move_baseline &&
          snapshot.last_generation != printed_generation) {
        received_after_move = true;
        printed_generation = snapshot.last_generation;
        latest_move_status = snapshot.status;
        has_latest_move_status = true;

        const auto now = std::chrono::steady_clock::now();
        const bool state_changed = !has_raw_state ||
                                   snapshot.status.motion_state != last_raw_motion_state ||
                                   snapshot.status.error_code != last_raw_error_code;
        if (state_changed || now - last_raw_print >= kStatusPrintInterval) {
          print_status("Raw status snapshot:", snapshot.last_generation, snapshot.status);
          last_raw_motion_state = snapshot.status.motion_state;
          last_raw_error_code = snapshot.status.error_code;
          last_raw_print = now;
          has_raw_state = true;
        }

        if (snapshot.status.error_code == 0 &&
            std::fabs(snapshot.status.position_mm - width_mm) <= kPositionToleranceMm &&
            std::fabs(snapshot.status.velocity_mm_s) <= kStoppedVelocityToleranceMmS) {
          std::cout << "Move completed within tolerance: requested=" << width_mm
                    << "mm actual=" << snapshot.status.position_mm
                    << "mm (verified by Raw status).\n";
          raw_target_reached = true;
          break;
        }
      }

      const auto sdk_state = robot.get_gripper_state(controller_group);
      if (sdk_state == nullptr) {
        continue;
      }
      const float position_mm = static_cast<float>(sdk_state->width * 1000.0);
      const float velocity_mm_s = static_cast<float>(sdk_state->velocity * 1000.0);
      const auto now = std::chrono::steady_clock::now();
      if (!has_sdk_position || std::fabs(position_mm - last_sdk_position_mm) >= 0.1F ||
          now - last_sdk_print >= kStatusPrintInterval) {
        std::cout << "SDK state: position=" << position_mm << "mm velocity=" << velocity_mm_s
                  << "mm/s effort=" << sdk_state->effort
                  << " is_moving=" << std::boolalpha << sdk_state->is_moving << std::noboolalpha
                  << '\n';
        last_sdk_position_mm = position_mm;
        last_sdk_print = now;
        has_sdk_position = true;
      }
      if (std::fabs(position_mm - width_mm) <= kPositionToleranceMm && !sdk_state->is_moving) {
        std::cout << "Move completed within tolerance: requested=" << width_mm
                  << "mm actual=" << position_mm
                  << "mm (verified by get_gripper_state).\n";
        sdk_target_reached = true;
        break;
      }
    }

    const StateSnapshot snapshot = get_snapshot(state);
    if (!received_after_move) {
      std::cout << "WARNING: no valid gripper status snapshot was received after Move. "
                   "Publication success alone does not prove that the device executed the "
                   "command.\n";
    } else if (steady_now_ms() - snapshot.last_update_ms >
               static_cast<uint64_t>(
                   std::chrono::duration_cast<std::chrono::milliseconds>(kStatusStaleInterval).count())) {
      std::cout << "WARNING: the valid gripper status stream stopped; subsequent receive "
                   "buffers were idle or rejected.\n";
    }
    print_rx_summary(snapshot);

    if (!raw_target_reached && !sdk_target_reached) {
      // Raw verification is authoritative for raw-mode operation. Retain the
      // SDK state as a fallback for configurations that still publish it.
      const auto sdk_state = robot.get_gripper_state(controller_group);
      if (sdk_state != nullptr) {
        const float position_mm = static_cast<float>(sdk_state->width * 1000.0);
        sdk_target_reached = std::fabs(position_mm - width_mm) <= kPositionToleranceMm &&
                             !sdk_state->is_moving;
      }

      if (!sdk_target_reached) {
        std::cerr << "FAILED: self-developed gripper verification failed: Move did not reach "
                     "the requested width: requested="
                  << width_mm << "mm tolerance=" << kPositionToleranceMm << "mm; ";
        if (sdk_state == nullptr) {
          std::cerr << "SDK state=unavailable; ";
        } else {
          const float position_mm = static_cast<float>(sdk_state->width * 1000.0);
          std::cerr << "SDK state={position=" << position_mm
                    << "mm,error=" << std::fabs(position_mm - width_mm)
                    << "mm,is_moving=" << std::boolalpha << sdk_state->is_moving
                    << std::noboolalpha << "}; ";
        }
        if (!has_latest_move_status) {
          std::cerr << "last_raw_status=none\n";
        } else {
          std::cerr << "last_raw_status={generation=" << printed_generation
                    << ",position=" << latest_move_status.position_mm
                    << "mm,velocity=" << latest_move_status.velocity_mm_s
                    << "mm/s,motion_state="
                    << static_cast<unsigned int>(latest_move_status.motion_state)
                    << ",error=0x" << std::hex
                    << static_cast<unsigned int>(latest_move_status.error_code) << std::dec
                    << "}\n";
        }
        success = false;
      }
    }
  }

  robot.unregister_endtool_raw_callback(callback_handle);
  if (restore_controller_value == 1) {
    robot.start_controller(controller_group);
  }
  robot.destroy();
  return success ? 0 : 4;
}

大寰二指 CAN 夹爪

通过 Raw CAN 传输配置和运动大寰二指夹爪，并主动轮询设备状态。

examples/s1/cpp/galbot_robot/src/endtool_raw_dahuan2_can_example.cpp

#include <array>
#include <atomic>
#include <chrono>
#include <cstdint>
#include <cstring>
#include <iomanip>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr uint32_t kTxCanId = 0x601;
constexpr uint32_t kRxCanId = 0x201;
constexpr uint8_t kWriteSingleRegister = 0x06;
constexpr uint8_t kReadRegisters = 0x03;
constexpr uint16_t kRegisterInit = 0x0100;
constexpr uint16_t kRegisterForce = 0x0101;
constexpr uint16_t kRegisterTargetPosition = 0x0103;
constexpr uint16_t kRegisterSpeed = 0x0104;
constexpr uint16_t kRegisterStatusBase = 0x0200;

struct Dahuan2Status {
  uint16_t init_status = 0;
  uint16_t grip_status = 0;
  uint16_t position_permille = 0;
  uint16_t speed = 0;
  int16_t current_ma = 0;
  uint16_t error = 0;
};

struct DemoState {
  std::mutex mutex;
  Dahuan2Status status;
  bool has_status = false;
  std::array<uint64_t, 2> generation_by_kind{};
  uint64_t valid_count = 0;
  uint64_t idle_snapshot_count = 0;
  uint64_t rejected_count = 0;
  uint64_t non_increasing_generation_count = 0;
};

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> make_request(
    uint8_t function, uint16_t address, uint16_t value) {
  std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> frame{};
  const uint32_t frame_type = 1;
  std::memcpy(frame.data(), &frame_type, sizeof(frame_type));
  std::memcpy(frame.data() + 4, &kTxCanId, sizeof(kTxCanId));
  frame[8] = 7;  // Five protocol bytes plus the two-byte TIB overhead.
  frame[12] = function;
  frame[13] = static_cast<uint8_t>(address >> 8);
  frame[14] = static_cast<uint8_t>(address);
  frame[15] = static_cast<uint8_t>(value >> 8);
  frame[16] = static_cast<uint8_t>(value);
  return frame;
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_init(bool calibrate) {
  return make_request(kWriteSingleRegister, kRegisterInit, calibrate ? 0x00A5 : 0x0001);
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_status_request() {
  return make_request(kReadRegisters, kRegisterStatusBase, 6);
}

bool decode_status(const EndToolRawData& raw, EndToolSide expected_side, Dahuan2Status& status) {
  // Dahuan CAN responses may appear in either receive buffer, so do not filter raw.kind.
  if (raw.side != expected_side) {
    return false;
  }
  // The receive PDO is a snapshot.  A zero marker means no new CAN response
  // arrived in this cycle; the remaining bytes may still contain an old reply.
  if (raw.frame[12] != 0x01) {
    return false;
  }

  uint32_t frame_type = 0;
  uint32_t can_id = 0;
  std::memcpy(&frame_type, raw.frame.data(), sizeof(frame_type));
  std::memcpy(&can_id, raw.frame.data() + 4, sizeof(can_id));
  if (frame_type != 1 || can_id != kRxCanId || raw.frame[16] != kReadRegisters ||
      raw.frame[17] < 12) {
    return false;
  }

  const auto read_be16 = [&raw](std::size_t register_offset) {
    const std::size_t offset = 18 + register_offset * 2;
    return static_cast<uint16_t>((static_cast<uint16_t>(raw.frame[offset]) << 8) |
                                 raw.frame[offset + 1]);
  };
  status.init_status = read_be16(0);
  status.grip_status = read_be16(1);
  status.position_permille = read_be16(2);
  status.speed = read_be16(3);
  status.current_ma = static_cast<int16_t>(read_be16(4));
  status.error = read_be16(5);
  return true;
}

const char* error_text(uint16_t error) {
  switch (error) {
    case 0:
      return "no error";
    case 1:
      return "under voltage";
    case 2:
      return "over voltage";
    case 3:
      return "over current";
    case 4:
      return "over heat";
    case 5:
      return "motor disconnected";
    case 8:
      return "overload";
    case 11:
      return "over speed";
    case 15:
      return "startup error";
    case 32:
      return "encoder error";
    default:
      return "unknown error";
  }
}

bool parse_side(const std::string& value, EndToolSide& side) {
  if (value == "left" || value == "l" || value == "0") {
    side = EndToolSide::LEFT;
    return true;
  }
  if (value == "right" || value == "r" || value == "1") {
    side = EndToolSide::RIGHT;
    return true;
  }
  return false;
}

uint64_t steady_now_ms() {
  return static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                   std::chrono::steady_clock::now().time_since_epoch())
                                   .count());
}

}  // namespace

int main(int argc, char** argv) {
  EndToolSide side = EndToolSide::LEFT;
  int position = 500;
  int speed = 50;
  int force = 50;
  int calibrate_value = 0;
  int duration_s = 10;
  int init_wait_s = 4;
  int run_init_value = 0;
  int stop_controller_value = 0;
  int restore_controller_value = 0;
  try {
    if (!parse_side(argc > 1 ? argv[1] : "left", side)) {
      std::cerr << "Side must be left/right/l/r/0/1.\n";
      return 1;
    }
    position = argc > 2 ? std::stoi(argv[2]) : position;
    speed = argc > 3 ? std::stoi(argv[3]) : speed;
    force = argc > 4 ? std::stoi(argv[4]) : force;
    calibrate_value = argc > 5 ? std::stoi(argv[5]) : calibrate_value;
    duration_s = argc > 6 ? std::stoi(argv[6]) : duration_s;
    init_wait_s = argc > 7 ? std::stoi(argv[7]) : init_wait_s;
    run_init_value = argc > 8 ? std::stoi(argv[8]) : run_init_value;
    stop_controller_value = argc > 9 ? std::stoi(argv[9]) : stop_controller_value;
    restore_controller_value = argc > 10 ? std::stoi(argv[10]) : restore_controller_value;
  } catch (const std::exception&) {
    std::cerr << "Invalid numeric argument.\n";
    return 1;
  }
  const bool calibrate = calibrate_value == 1;
  const bool run_init = calibrate || run_init_value == 1;
  if (position < 0 || position > 1000 || speed < 1 || speed > 100 || force < 20 ||
      force > 100 || (calibrate_value != 0 && calibrate_value != 1) || duration_s < 0 ||
      init_wait_s < 0 || (run_init_value != 0 && run_init_value != 1) ||
      (stop_controller_value != 0 && stop_controller_value != 1) ||
      (restore_controller_value != 0 && restore_controller_value != 1)) {
    std::cerr << "Usage: " << argv[0]
              << " [left|right] [position:0..1000] [speed:1..100] [force:20..100]"
                 " [calibrate:0|1] [duration_s] [init_wait_s]"
                 " [run_init:0|1] [stop_standard_controller:0|1]"
                 " [restore_controller:0|1]\n";
    return 1;
  }

  const std::string controller_group = side == EndToolSide::LEFT ? "left_gripper" : "right_gripper";
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  if (!robot.init()) {
    std::cerr << "FAILED: failed to initialize the S1 SDK.\n";
    return 2;
  }

  std::cout << "WARNING: raw TX has no server-side exclusive writer lock.\n";
  std::cout << "WBCS must run with endtool_raw_enabled=true.\n";
  if (stop_controller_value == 1) {
    std::cout << "Best-effort stop of " << controller_group << ": "
              << static_cast<int>(robot.stop_controller(controller_group)) << '\n';
  }

  auto last_print_ms = std::make_shared<std::atomic<uint64_t>>(0);
  auto state = std::make_shared<DemoState>();
  const uint64_t callback_handle = robot.register_endtool_raw_callback(
      [side, last_print_ms, state](const EndToolRawData& raw) {
        if (raw.side != side) {
          return;
        }
        if (raw.frame[12] != 0x01) {
          std::lock_guard<std::mutex> lock(state->mutex);
          ++state->idle_snapshot_count;
          return;
        }
        Dahuan2Status status;
        if (!decode_status(raw, side, status)) {
          std::lock_guard<std::mutex> lock(state->mutex);
          ++state->rejected_count;
          return;
        }
        {
          std::lock_guard<std::mutex> lock(state->mutex);
          const std::size_t kind_index =
              raw.kind == EndToolRxKind::BUFFER_1KHZ ? 0U : 1U;
          const uint64_t previous_generation = state->generation_by_kind[kind_index];
          if (raw.generation != 0 && previous_generation != 0 &&
              raw.generation <= previous_generation) {
            ++state->non_increasing_generation_count;
            return;
          }
          state->generation_by_kind[kind_index] = raw.generation;
          state->status = status;
          state->has_status = true;
          ++state->valid_count;
        }
        const uint64_t now_ms = steady_now_ms();
        uint64_t previous = last_print_ms->load(std::memory_order_relaxed);
        if (now_ms - previous < 200 || !last_print_ms->compare_exchange_strong(
                                           previous, now_ms, std::memory_order_relaxed)) {
          return;
        }
        std::cout << "RX kind=" << static_cast<uint32_t>(raw.kind)
                  << " generation=" << raw.generation << " init=" << status.init_status
                  << " grip=" << status.grip_status << " position=" << status.position_permille
                  << "permille speed=" << status.speed << " current=" << status.current_ma
                  << "mA error=0x" << std::hex << status.error << std::dec << " ("
                  << error_text(status.error) << ")\n";
      });
  if (callback_handle == 0) {
    std::cerr << "FAILED: failed to register the raw RX callback.\n";
    if (restore_controller_value == 1) {
      robot.start_controller(controller_group);
    }
    robot.destroy();
    return 3;
  }

  const auto publish = [&robot, side](const auto& frame, const char* description) {
    const auto result = robot.send_endtool_raw_frame(side, frame);
    if (result != ControlStatus::SUCCESS) {
      std::cerr << "FAILED: failed to publish " << description << ": "
                << static_cast<int>(result) << '\n';
      return false;
    }
    return true;
  };
  const auto poll_for = [&publish](int seconds) {
    const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(seconds);
    while (std::chrono::steady_clock::now() < deadline) {
      if (!publish(encode_status_request(), "status request")) {
        return false;
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
    return true;
  };

  bool success = true;
  if (run_init) {
    success = publish(encode_init(calibrate), calibrate ? "calibration command" : "homing command");
  }
  if (success && run_init) {
    std::cout << "Polling initialization state for " << init_wait_s << " seconds...\n";
    success = poll_for(init_wait_s);
  }
  success = success && publish(make_request(kWriteSingleRegister, kRegisterSpeed, speed), "speed command");
  success = success && publish(make_request(kWriteSingleRegister, kRegisterForce, force), "force command");
  success = success && publish(
                           make_request(kWriteSingleRegister, kRegisterTargetPosition, position),
                           "position command");
  uint64_t motion_status_baseline = 0;
  if (success) {
    std::lock_guard<std::mutex> lock(state->mutex);
    // Drop any response left by initialization polling. The following active
    // reads must produce a fresh status before the target can be verified.
    state->has_status = false;
    motion_status_baseline = state->valid_count;
  }
  if (success) {
    std::cout << "Polling status for " << duration_s << " seconds...\n";
    success = poll_for(duration_s);
  }

  if (success) {
    Dahuan2Status status;
    bool has_status = false;
    uint64_t valid_count = 0;
    uint64_t idle_count = 0;
    uint64_t rejected_count = 0;
    uint64_t non_increasing_count = 0;
    {
      std::lock_guard<std::mutex> lock(state->mutex);
      status = state->status;
      has_status = state->has_status;
      valid_count = state->valid_count;
      idle_count = state->idle_snapshot_count;
      rejected_count = state->rejected_count;
      non_increasing_count = state->non_increasing_generation_count;
    }
    if (!has_status || valid_count <= motion_status_baseline) {
      std::cerr << "FAILED: Dahuan2 device verification failed: no fresh status response was "
                   "received: idle_snapshots="
                << idle_count
                << " rejected=" << rejected_count
                << " non_increasing_generations=" << non_increasing_count << '\n';
      success = false;
    } else if (status.error != 0) {
      std::cerr << "FAILED: Dahuan2 device verification failed: gripper status error=0x"
                << std::hex << status.error << std::dec
                << " (" << error_text(status.error) << ")\n";
      success = false;
    } else if (std::abs(static_cast<int>(status.position_permille) - position) > 20) {
      std::cerr << "FAILED: Dahuan2 device verification failed: position did not reach target: "
                   "requested="
                << position
                << " actual=" << status.position_permille << " tolerance=20permille\n";
      success = false;
    } else {
      std::cout << "Position reached: requested=" << position
                << " actual=" << status.position_permille
                << "permille fresh_status_count=" << valid_count << '\n';
    }
  }

  robot.unregister_endtool_raw_callback(callback_handle);
  if (restore_controller_value == 1) {
    robot.start_controller(controller_group);
  }
  robot.destroy();
  return success ? 0 : 4;
}

坤维 RS485 六维力传感器

以不追赶突发的方式周期发送 RS485 查询，并从 1 kHz 接收流校验、监控和打印六维力/力矩数据。

examples/s1/cpp/galbot_robot/src/endtool_raw_kunwei_rs485_example.cpp

#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr float kMaxAbsWrenchValue = 1.0e6F;
constexpr auto kStaleTimeout = std::chrono::milliseconds(1000);
constexpr auto kFreezeTimeout = std::chrono::milliseconds(2000);
constexpr auto kWarningInterval = std::chrono::milliseconds(1000);
constexpr auto kStatsInterval = std::chrono::seconds(5);

struct ForceReading {
  float fx = 0.0F;
  float fy = 0.0F;
  float fz = 0.0F;
  float mx = 0.0F;
  float my = 0.0F;
  float mz = 0.0F;
};

enum class DecodeResult {
  OK,
  PREFIX_MISMATCH,
  TERMINATOR_MISMATCH,
  INVALID_NUMERIC,
  ALL_ZERO,
};

struct DemoState {
  std::atomic<uint64_t> query_ok{0};
  std::atomic<uint64_t> query_fail{0};
  std::atomic<uint64_t> rx_1khz{0};
  std::atomic<uint64_t> rejected{0};
  std::atomic<uint64_t> reject_prefix{0};
  std::atomic<uint64_t> reject_terminator{0};
  std::atomic<uint64_t> reject_numeric{0};
  std::atomic<uint64_t> reject_all_zero{0};
  std::atomic<uint64_t> non_increasing_generation{0};
  std::atomic<uint64_t> duplicate_snapshots{0};
  std::atomic<uint64_t> valid{0};
  std::atomic<uint64_t> last_print_ms{0};
  std::atomic<uint64_t> last_valid_ms{0};
  std::atomic<uint64_t> last_generation{0};
  std::atomic<uint64_t> last_payload_change_ms{0};
  std::atomic<uint64_t> last_freeze_warning_ms{0};
  std::atomic<uint64_t> last_stale_warning_ms{0};
  std::mutex payload_mutex;
  std::array<uint8_t, 24> last_payload{};
  bool has_last_payload = false;
};

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_query() {
  std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> frame{};
  const uint32_t frame_type = 2;
  const uint32_t device_id = 233;
  const uint32_t data_size = 4;
  std::memcpy(frame.data(), &frame_type, sizeof(frame_type));
  std::memcpy(frame.data() + 4, &device_id, sizeof(device_id));
  std::memcpy(frame.data() + 8, &data_size, sizeof(data_size));
  frame[12] = 0x49;
  frame[13] = 0xAA;
  frame[14] = 0x0D;
  frame[15] = 0x0A;
  return frame;
}

DecodeResult decode_force(
    const std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE>& frame, ForceReading& reading) {
  if ((frame[20] != 0x48 && frame[20] != 0x49) || frame[21] != 0xAA) {
    return DecodeResult::PREFIX_MISMATCH;
  }
  if (frame[46] != 0x0D || frame[47] != 0x0A) {
    return DecodeResult::TERMINATOR_MISMATCH;
  }

  ForceReading decoded;
  std::memcpy(&decoded.fx, frame.data() + 22, sizeof(float));
  std::memcpy(&decoded.fy, frame.data() + 26, sizeof(float));
  std::memcpy(&decoded.fz, frame.data() + 30, sizeof(float));
  std::memcpy(&decoded.mx, frame.data() + 34, sizeof(float));
  std::memcpy(&decoded.my, frame.data() + 38, sizeof(float));
  std::memcpy(&decoded.mz, frame.data() + 42, sizeof(float));

  const float values[] = {decoded.fx, decoded.fy, decoded.fz,
                          decoded.mx, decoded.my, decoded.mz};
  bool all_zero = true;
  for (const float value : values) {
    if (!std::isfinite(value) || std::fabs(value) > kMaxAbsWrenchValue) {
      return DecodeResult::INVALID_NUMERIC;
    }
    all_zero = all_zero && value == 0.0F;
  }
  if (all_zero) {
    return DecodeResult::ALL_ZERO;
  }

  reading = decoded;
  return DecodeResult::OK;
}

bool parse_side(const std::string& value, EndToolSide& side) {
  if (value == "left" || value == "l" || value == "0") {
    side = EndToolSide::LEFT;
    return true;
  }
  if (value == "right" || value == "r" || value == "1") {
    side = EndToolSide::RIGHT;
    return true;
  }
  return false;
}

uint64_t steady_now_ms() {
  return static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                   std::chrono::steady_clock::now().time_since_epoch())
                                   .count());
}

void count_decode_rejection(const std::shared_ptr<DemoState>& state, DecodeResult result) {
  state->rejected.fetch_add(1, std::memory_order_relaxed);
  switch (result) {
    case DecodeResult::PREFIX_MISMATCH:
      state->reject_prefix.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::TERMINATOR_MISMATCH:
      state->reject_terminator.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::INVALID_NUMERIC:
      state->reject_numeric.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::ALL_ZERO:
      state->reject_all_zero.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::OK:
      break;
  }
}

}  // namespace

int main(int argc, char** argv) {
  EndToolSide side = EndToolSide::LEFT;
  int duration_s = 10;
  int query_interval_ms = 10;
  int print_interval_ms = 100;
  try {
    if (!parse_side(argc > 1 ? argv[1] : "left", side)) {
      std::cerr << "Side must be left/right/l/r/0/1.\n";
      return 1;
    }
    duration_s = argc > 2 ? std::stoi(argv[2]) : duration_s;
    query_interval_ms = argc > 3 ? std::stoi(argv[3]) : query_interval_ms;
    print_interval_ms = argc > 4 ? std::stoi(argv[4]) : print_interval_ms;
  } catch (const std::exception&) {
    std::cerr << "Invalid numeric argument.\n";
    return 1;
  }
  if (duration_s <= 0 || query_interval_ms <= 0 || print_interval_ms <= 0) {
    std::cerr << "Usage: " << argv[0]
              << " [left|right] [duration_s>0] [query_interval_ms>0] [print_interval_ms>0]\n";
    return 1;
  }

  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  if (!robot.init()) {
    std::cerr << "Failed to initialize the S1 SDK.\n";
    return 2;
  }
  std::cout << "WARNING: same-process raw TX is serialized, but there is no cross-client "
               "resource lease.\n";
  std::cout << "WBCS must run with endtool_raw_enabled=true.\n";

  auto state = std::make_shared<DemoState>();
  const uint64_t callback_handle = robot.register_endtool_raw_callback(
      [side, print_interval_ms, state](const EndToolRawData& raw) {
        if (raw.side != side || raw.kind != EndToolRxKind::BUFFER_1KHZ) {
          return;
        }
        state->rx_1khz.fetch_add(1, std::memory_order_relaxed);

        ForceReading reading;
        const DecodeResult decode_result = decode_force(raw.frame, reading);
        if (decode_result != DecodeResult::OK) {
          count_decode_rejection(state, decode_result);
          return;
        }

        uint64_t previous_generation = state->last_generation.load(std::memory_order_relaxed);
        while (true) {
          if (previous_generation != 0 && raw.generation <= previous_generation) {
            state->non_increasing_generation.fetch_add(1, std::memory_order_relaxed);
            return;
          }
          if (state->last_generation.compare_exchange_weak(
                  previous_generation, raw.generation, std::memory_order_relaxed,
                  std::memory_order_relaxed)) {
            break;
          }
        }

        const uint64_t now_ms = steady_now_ms();
        bool duplicate = false;
        {
          std::lock_guard<std::mutex> lock(state->payload_mutex);
          duplicate = state->has_last_payload &&
                      std::equal(state->last_payload.begin(), state->last_payload.end(),
                                 raw.frame.begin() + 22);
          if (!duplicate) {
            std::copy(raw.frame.begin() + 22, raw.frame.begin() + 46,
                      state->last_payload.begin());
            state->has_last_payload = true;
          }
        }
        if (!duplicate) {
          state->last_payload_change_ms.store(now_ms, std::memory_order_relaxed);
        } else {
          state->duplicate_snapshots.fetch_add(1, std::memory_order_relaxed);
          const uint64_t last_change_ms =
              state->last_payload_change_ms.load(std::memory_order_relaxed);
          uint64_t last_warning_ms =
              state->last_freeze_warning_ms.load(std::memory_order_relaxed);
          if (last_change_ms != 0 &&
              now_ms - last_change_ms >= static_cast<uint64_t>(kFreezeTimeout.count()) &&
              (last_warning_ms == 0 ||
               now_ms - last_warning_ms >= static_cast<uint64_t>(kWarningInterval.count())) &&
              state->last_freeze_warning_ms.compare_exchange_strong(
                  last_warning_ms, now_ms, std::memory_order_relaxed,
                  std::memory_order_relaxed)) {
            std::cerr << "Force payload unchanged for " << (now_ms - last_change_ms)
                      << "ms while generation advances. This may be a held TIB snapshot; an "
                         "exactly steady sensor can also produce identical bytes.\n";
          }
          return;
        }

        state->last_valid_ms.store(now_ms, std::memory_order_relaxed);
        state->valid.fetch_add(1, std::memory_order_relaxed);
        uint64_t previous_print = state->last_print_ms.load(std::memory_order_relaxed);
        if (now_ms - previous_print < static_cast<uint64_t>(print_interval_ms) ||
            !state->last_print_ms.compare_exchange_strong(
                previous_print, now_ms, std::memory_order_relaxed,
                std::memory_order_relaxed)) {
          return;
        }
        std::cout << "generation=" << raw.generation << " Fx=" << reading.fx
                  << " Fy=" << reading.fy << " Fz=" << reading.fz << "kg Mx=" << reading.mx
                  << " My=" << reading.my << " Mz=" << reading.mz << "kg*m\n";
      });
  if (callback_handle == 0) {
    std::cerr << "Failed to register the raw RX callback.\n";
    robot.destroy();
    return 3;
  }

  const auto query = encode_query();
  const auto query_interval = std::chrono::milliseconds(query_interval_ms);
  const auto started_at = std::chrono::steady_clock::now();
  const auto deadline = started_at + std::chrono::seconds(duration_s);
  auto next_query_at = started_at;
  auto next_stats_at = started_at + kStatsInterval;
  auto last_stats_at = started_at;
  uint64_t last_stats_query_count = 0;
  uint64_t last_stats_rx_count = 0;

  while (true) {
    const auto before_sleep = std::chrono::steady_clock::now();
    if (before_sleep < next_query_at) {
      std::this_thread::sleep_until(next_query_at);
    }
    if (std::chrono::steady_clock::now() >= deadline) {
      break;
    }

    const auto result = robot.send_endtool_raw_frame(side, query);
    if (result == ControlStatus::SUCCESS) {
      state->query_ok.fetch_add(1, std::memory_order_relaxed);
    } else {
      const uint64_t failures = state->query_fail.fetch_add(1, std::memory_order_relaxed) + 1;
      if (failures == 1 || failures % 100 == 0) {
        std::cerr << "Query publish failed: status=" << static_cast<int>(result)
                  << " failures=" << failures << '\n';
      }
    }

    const auto after_query = std::chrono::steady_clock::now();
    next_query_at += query_interval;
    // Never catch up with a burst after Publish blocks; doing so only increases FIFO pressure.
    if (next_query_at <= after_query) {
      next_query_at = after_query + query_interval;
    }

    const uint64_t now_ms = steady_now_ms();
    const uint64_t last_valid_ms = state->last_valid_ms.load(std::memory_order_relaxed);
    const uint64_t stale_for_ms =
        last_valid_ms == 0
            ? static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                        after_query - started_at)
                                        .count())
            : now_ms - last_valid_ms;
    uint64_t last_stale_warning_ms =
        state->last_stale_warning_ms.load(std::memory_order_relaxed);
    if (stale_for_ms >= static_cast<uint64_t>(kStaleTimeout.count()) &&
        (last_stale_warning_ms == 0 ||
         now_ms - last_stale_warning_ms >= static_cast<uint64_t>(kWarningInterval.count())) &&
        state->last_stale_warning_ms.compare_exchange_strong(
            last_stale_warning_ms, now_ms, std::memory_order_relaxed,
            std::memory_order_relaxed)) {
      std::cerr << "No fresh force response for " << stale_for_ms
                << "ms: query_ok=" << state->query_ok.load(std::memory_order_relaxed)
                << " query_fail=" << state->query_fail.load(std::memory_order_relaxed)
                << " rx_1khz=" << state->rx_1khz.load(std::memory_order_relaxed)
                << " valid=" << state->valid.load(std::memory_order_relaxed)
                << ". Check TIB, sensor power/wiring/baud rate, "
                   "endtool_raw_enabled and WBCS logs.\n";
    }

    if (after_query >= next_stats_at) {
      const uint64_t query_count = state->query_ok.load(std::memory_order_relaxed);
      const uint64_t rx_count = state->rx_1khz.load(std::memory_order_relaxed);
      const double elapsed_s = std::chrono::duration<double>(after_query - last_stats_at).count();
      std::cout << "Rate: query_hz="
                << static_cast<double>(query_count - last_stats_query_count) / elapsed_s
                << " rx_hz=" << static_cast<double>(rx_count - last_stats_rx_count) / elapsed_s
                << " fresh=" << state->valid.load(std::memory_order_relaxed)
                << " duplicates=" << state->duplicate_snapshots.load(std::memory_order_relaxed)
                << " rejects(prefix=" << state->reject_prefix.load(std::memory_order_relaxed)
                << " terminator=" << state->reject_terminator.load(std::memory_order_relaxed)
                << " numeric=" << state->reject_numeric.load(std::memory_order_relaxed)
                << " zero=" << state->reject_all_zero.load(std::memory_order_relaxed) << ")\n";
      last_stats_query_count = query_count;
      last_stats_rx_count = rx_count;
      last_stats_at = after_query;
      next_stats_at = after_query + kStatsInterval;
    }
  }

  robot.unregister_endtool_raw_callback(callback_handle);
  std::cout << "Stopped: query_ok=" << state->query_ok.load()
            << " query_fail=" << state->query_fail.load()
            << " rx_1khz=" << state->rx_1khz.load()
            << " rejected=" << state->rejected.load()
            << " (prefix=" << state->reject_prefix.load()
            << " terminator=" << state->reject_terminator.load()
            << " numeric=" << state->reject_numeric.load()
            << " zero=" << state->reject_all_zero.load()
            << " non_increasing_generation=" << state->non_increasing_generation.load() << ")"
            << " fresh=" << state->valid.load()
            << " duplicates=" << state->duplicate_snapshots.load() << '\n';
  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::S1);

    // 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::S1)
                            .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::S1);

    // 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 names to control, including ["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"] (S1: torso replaces G1 leg)
    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;

    // Execute joint trajectory
    ControlStatus status = robot.execute_joint_trajectory(trajectory, is_traj_block);
    if (status == ControlStatus::SUCCESS) {
        std::cout << "Trajectory execution succeeded!" << std::endl;
    } else {
        std::cerr << "Trajectory execution failed!" << std::endl;
    }

    // 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::S1);

    // 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::S1);

  // 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 ["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"] (S1: torso replaces G1 leg)
  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 S1 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 torso / 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`
 *    - On S1 this corresponds to the `swerve_chassis` task
 *    - 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(S1ControllerName::SWERVE_CHASSIS_POSE_CTRL)` or
 *      `switch_controller(S1ControllerName::SWERVE_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 = S1JointGroup::SWERVE_CHASSIS;
constexpr const char* kChassisSubtaskPose = "swerve_chassis_pose";
constexpr const char* kChassisSubtaskTwist = "swerve_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 torso target\n"
            << "  base_pose    - publish a swerve chassis pose target\n"
            << "  base_twist   - publish a swerve chassis twist target with auto stop\n"
            << "  mixed_pose   - publish torso + swerve chassis pose in one target\n"
            << "  mixed_twist  - publish torso + swerve 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::S1);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

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

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

  const std::vector<std::string> torso_single_joint = {torso_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;
  constexpr double kTorsoJt = 0.60; // torso target height,  meter
  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(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS);
      print_result("joint", robot.PublishTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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 S1 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 torso / 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`
 *    - On S1 this corresponds to the `swerve_chassis` task
 *    - 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(S1ControllerName::SWERVE_CHASSIS_POSE_CTRL)` or
 *      `switch_controller(S1ControllerName::SWERVE_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 = S1JointGroup::SWERVE_CHASSIS;
constexpr const char* kChassisSubtaskPose = "swerve_chassis_pose";
constexpr const char* kChassisSubtaskTwist = "swerve_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 torso target\n"
            << "  base_pose    - request a swerve chassis pose target\n"
            << "  base_twist   - request a swerve chassis twist target with auto stop\n"
            << "  mixed_pose   - request torso + swerve chassis pose in one target\n"
            << "  mixed_twist  - request torso + swerve 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::S1);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

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

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

  const std::vector<std::string> torso_single_joint = {torso_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;
  constexpr double kTorsoJt = 0.60; // torso target height,  meter

  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(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS);
      print_error_info("joint", robot.RequestTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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::S1);

    // 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::S1);

    // 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 << "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 << "------------------" << std::endl;
    }
}

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

    // 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::S1);

    // 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::S1);

    // 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 ["torso", "head", "left_arm", "right_arm"] (S1: torso replaces G1 leg)
    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::S1);

    // 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_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::S1);

    // 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::S1);

    // Available IMU sensors on S1 (choose one of the following three):
    // - SensorType::HEAD_IMU: Head lidar IMU
    // - SensorType::BACK_IMU: Rear lidar IMU
    // - SensorType::CHASSIS_IMU: Chassis lidar IMU
    // Initialize sensors
    std::unordered_set<SensorType> sensor_types = {
        SensorType::HEAD_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
    std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::HEAD_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_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::S1);

  // 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::S1);

    // 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(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 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::S1);

  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::S1);

    // 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::S1);

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    // Available lidar sensors on S1 (choose one of the following three):
    // - SensorType::HEAD_LIDAR: Head lidar
    // - SensorType::BACK_LIDAR: Rear lidar
    // - SensorType::CHASSIS_LIDAR: Chassis lidar
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_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::HEAD_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::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::S1);
  std::unordered_set<SensorType> enable_sensor_set = {
      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
  };
  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::S1);
    auto& motion = GalbotMotion::get_instance(MachineType::S1);

    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
    // FIXME:EXAMPLE[9]: Parameter names contain "leg" (G1), verify S1 parameter names
    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::S1);
    auto& motion = GalbotMotion::get_instance(MachineType::S1);

    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
    // FIXME:EXAMPLE[10]: Parameter names contain "leg" (G1), verify S1 parameter names
    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;
}

控制器管理

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

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::S1);

  // 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 = S1ControllerName::LEFT_ARM_PVT_CTRL;
  const std::string controller_str = S1ControllerName::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_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::S1);

    // 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;
}

安装验证

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

examples/cpp/galbot_robot/src/installation_welcome_verify.cpp

#include <chrono>
#include <condition_variable>
#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 = 35.0;
constexpr auto kVideoStreamTimeout = std::chrono::seconds(10);

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> kPresetTorso = {0.58};
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.7, 0.0};
const std::vector<double> kPresetRightArm = {-2.0, 1.5, 0.6, 1.7, 0.0, 0.7, 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. Torso 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;
}

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 head_video_stream_ok = false;

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

  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 (S1, HEAD_LEFT_CAMERA enabled)" << std::endl;
  std::this_thread::sleep_for(std::chrono::duration<double>(5.0));

  bool torso_step_ok = false;
  bool upper_step_ok = false;
  bool demo_step_ok = false;

  const ControlStatus torso_set_status =
      robot.set_joint_positions(kPresetTorso, {"torso"}, {}, true, kSpeedRadS, kTimeoutS);
  torso_step_ok = (torso_set_status == ControlStatus::SUCCESS);
  if (!torso_step_ok) {
    std::cerr << "[FAIL] Torso set_joint_positions not SUCCESS (blocking)" << std::endl;
  }
  print_summary("Torso set_joint_positions (blocking)", torso_step_ok);

  if (torso_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: torso 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 = torso_step_ok && upper_step_ok && demo_step_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("Head camera H.264 video stream", head_video_stream_ok);

  const bool installation_all_checks_passed = body_preset_step_passed && 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_base_pose）

适用场景：控制移动底盘在指定参考坐标系下运动至目标位姿。

examples/cpp/galbot_robot/src/set_base_pose_example.cpp

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

#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper function: calculate quaternion
void set_yaw_orientation(Pose& pose, double yaw) {
    const double half_yaw = 0.5 * yaw;
    pose.orientation.x = 0.0;
    pose.orientation.y = 0.0;
    pose.orientation.z = std::sin(half_yaw);
    pose.orientation.w = std::cos(half_yaw);
}

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

    // 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));

    // ========== Test Version 1: Use Pose struct ==========
    std::cout << "\n[Test 1] Set chassis pose using Pose struct..." << std::endl;
    {
        Pose pose;
        pose.position.x = 0.3;    // Move to x = 0.3 m in the odom frame
        pose.position.y = 0.0;    // Target y coordinate
        pose.position.z = 0.0;    // z coordinate (chassis ignored)
        set_yaw_orientation(pose, 0.0);  // Oriented to 0 radians

        if (robot.set_base_pose(pose, true, 15.0) == ControlStatus::SUCCESS) {
            std::cout << "[Test 1] Pose-struct set succeeded, target position reached." << std::endl;
        } else {
            std::cerr << "[Test 1] Pose-struct set failed or timed out." << std::endl;
        }
    }

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

    // ========== Test Version 2: Simplified coordinate version (default 1s motion time) ==========
    std::cout << "\n[Test 2] Use simplified coordinate version (default 1s motion time)..." << std::endl;
    {
        double x = -0.3;          // Move backward by 0.3 m from the current pose
        double y = 0.0;           // Target y coordinate
        double yaw = 0.0;         // Target orientation
        // std::string frame_id = "base_link";
        std::string frame_id = "rel(0)";
        std::string reference_frame_id = "odom";
        bool is_blocking = true;
        double timeout_s = 15.0;

        if (robot.set_base_pose(x, y, yaw, frame_id, reference_frame_id, is_blocking, timeout_s) == ControlStatus::SUCCESS) {
            std::cout << "[Test 2] Simplified-coordinate set succeeded, target position reached." << std::endl;
        } else {
            std::cerr << "[Test 2] Simplified-coordinate set failed or timed out." << std::endl;
        }
    }

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

    // ========== Test Version 3: Full control version (custom motion time) ==========
    std::cout << "\n[Test 3] Use full control version (custom 5s motion time)..." << std::endl;
    {
        double x = 0.0;           // Target x coordinate (return to origin)
        double y = 0.0;           // Target y coordinate
        double yaw = 0.0;         // Target orientation
        // std::string frame_id = "base_link";
        std::string frame_id = "rel(0)";
        std::string reference_frame_id = "odom";
        double time_from_start_s = 5.0;  // Custom 5-second motion (slower and more stable)
        bool is_blocking = true;
        double timeout_s = 15.0;

        if (robot.set_base_pose(x, y, yaw, frame_id, reference_frame_id, time_from_start_s, is_blocking, timeout_s) == ControlStatus::SUCCESS) {
            std::cout << "[Test 3] Full-control set succeeded, target position reached." << std::endl;
        } else {
            std::cerr << "[Test 3] Full-control set failed or timed out." << std::endl;
        }
    }

    std::cout << "\nAll tests completed!" << std::endl;

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

    return 0;
}

设置机器人配置（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/s1/en/set_config_reference.md

using namespace galbot::sdk;

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

    // 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 and select the short-distance
    // motion mode. omni_plan is G1-only (passing it on S1 returns INVALID_INPUT), so S1
    // uses adjust_motion_type instead -- 2 = rotate + crab-walk, the recommended S1 mode
    // (avoids joint collisions).
    std::vector<ConfigItem> navigation_fields = {
        ConfigItem{"replan_threshold", 30.0},
        ConfigItem{"no_replan_threshold", 1.0},
        ConfigItem{"adjust_motion_type", static_cast<int64_t>(2)},
    };
    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) together with
    // leg trajectory velocity/acceleration/jerk limits. Per-chain fields are keyed as
    // "{field_name}_{chain_name}"; the leg chain has only 1 joint on S1 (5 on G1), so the
    // array length here is S1-specific.
    std::vector<ConfigItem> motion_plan_fields = {
        ConfigItem{"plan_timeout", 5.0},
        ConfigItem{"max_velocity_leg", std::vector<double>{0.8}},
        ConfigItem{"max_acceleration_leg", std::vector<double>{3.0}},
        ConfigItem{"max_jerk_leg", std::vector<double>{10.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 <vector>
#include "galbot_robot.hpp"
using namespace galbot::sdk;
int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  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);
  std::cout << "status: " << static_cast<int>(status) << ", fields: " << fields.size() << std::endl;
  fields.clear(); status = robot.get_config(ConfigService::CONTROL, keys, &fields, true);
  std::cout << "default status: " << static_cast<int>(status) << std::endl;
  robot.request_shutdown(); robot.wait_for_shutdown(); robot.destroy(); 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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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;
  }

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

  std::map<std::string, std::vector<double>> chain_joints = {
      {"torso", {1.1}},
      {"head", {0.0000, -0.26}},
      {"left_arm", {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
      {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}};

  std::vector<double> whole_body_joint;
  std::vector<std::string> keys = {"torso", "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);
  } 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_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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;
  // Test all kinematic chains; no chain is excluded.
  // torso/leg are virtual chains handled by the server (leg is treated the same as torso).
  if (support_chains.empty()) {
    support_chains = {"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.
  // S1's get_chain_joint_state() skips leg, but leg still accepts a joint value (DOF = 1).
  const std::unordered_map<std::string, size_t> fallback_dof = {{"leg", 1}};

  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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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;
  // torso/leg are virtual chains handled by the server (leg is treated the same as torso).
  if (support_chains.empty()) {
    support_chains = {"left_arm", "right_arm", "torso", "leg"};
  }

  const std::string target_frame = "EndEffector";
  const std::string reference_frame = "base_link";

  size_t whole_body_dof = 17;
  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;
}

获取连杆名称（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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  // 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 <chrono>
#include <iostream>
#include <memory>
#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_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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));

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

  // Get current end-effector pose from robot as target for subsequent IK scenarios
  std::vector<double> target_pose;
  {
    auto [status, pose] = planner.get_end_effector_pose_on_chain(target_chain, target_frame, reference_frame);
    if (status != MotionStatus::SUCCESS || pose.size() != 7) {
      std::cerr << "Failed to get current end-effector pose, cannot continue test." << std::endl;
      return -1;
    }
    target_pose = pose;
    std::cout << "Current " << target_chain << " End-effector pose: ";
    for (double v : target_pose)
      std::cout << v << " ";
    std::cout << std::endl;
    std::cout << "---------------------------------------------------" << std::endl;
  }

  // Scenario 1: Single-chain inverse kinematics (using default parameters)
  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(target_pose, one_chain);
    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(target_pose, 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 (expected to return INVALID_INPUT)
  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(target_pose, 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: Inverse kinematics with initial reference joint values (using current robot state)
  try {
    std::cout << ">> Running Scenario 4: IK test with initial reference values..." << std::endl;
    std::vector<std::string> one_chain = {target_chain};

    auto current_chain_joints = planner.get_chain_joint_state();
    std::cout << "  Current joint states:" << std::endl;
    for (const auto& [name, joints] : current_chain_joints) {
      std::cout << "    [" << name << "]: ";
      for (double v : joints)
        std::cout << v << " ";
      std::cout << std::endl;
    }

    auto res = planner.inverse_kinematics(target_pose, one_chain, target_frame, reference_frame, current_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: Inverse kinematics based on RobotStates (get full state from robot)
  try {
    std::cout << ">> Running Scenario 5: IK test based on RobotStates..." << std::endl;

    auto robot_states = planner.get_robot_states();
    auto ref_state = std::make_shared<RobotStates>(robot_states);
    ref_state->chain_name = target_chain;

    std::cout << "  Number of whole-body joints: " << ref_state->whole_body_joint.size() << std::endl;
    std::cout << "  Number of chassis states: " << ref_state->base_state.size() << std::endl;

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

    auto res = planner.inverse_kinematics_by_state(target_pose, one_chain, target_frame, reference_frame, ref_state,
                                                   false, params);
    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 <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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::string reference_frame = "base_link";
  std::string target_frame = "EndEffector";
  std::string target_chain = "right_arm";
  std::string end_ee_link = "right_arm_end_effector_mount_link";
  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;

    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 ---
  std::vector<double> current_pose;
  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);
    current_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", current_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 (small offset from current pose) ---
  try {
    std::cout << ">> Scenario 3: Setting end-effector pose..." << std::endl;

    if (current_pose.size() != 7) {
      std::cerr << "❌ Unable to get current pose; skipping Scenario 3" << std::endl;
    } else {
      std::vector<double> target_pose = current_pose;
      target_pose[2] -= 0.05;  // Move downward 5 cm in z direction

      print_pose_info("Target pose", target_pose);

      MotionStatus status = planner.set_end_effector_pose(target_pose,      // 1. target_pose
                                                          target_chain,     // 2. chain_name
                                                          reference_frame,  // 3. reference_frame
                                                          nullptr,          // 4. reference_robot_states
                                                          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 << "✅ Motion execution completed" << std::endl;
      }
    }
  } catch (const std::exception& e) {
    std::cerr << "❌ Pose-setting exception: " << e.what() << std::endl;
  }

  // --- Scenario 4: Get end-effector pose after execution to verify result ---
  try {
    std::cout << ">> Scenario 4: Getting end-effector pose after motion..." << std::endl;

    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("After motion", 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() {
    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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 = {
        {"torso",     {0.65}},
        {"head",      {0.0000, -0.26}},
        {"left_arm",  {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"torso", "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));

    std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
        {"torso",     {-0.0715, 0.0000, 1.2044, 0.0000, 0.0000, 0.0000, 1.0000}},
        {"head",      {-0.0123, 0.0046, 1.4975, -0.0002, 0.1298, -0.0001, 0.9915}},
        {"left_arm",  {0.3851, 0.4190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920}},
        {"right_arm", {0.2819, -0.3212, 1.9315, 0.3701, 0.7731, 0.1567, -0.4907}}
    };

    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];
        target_joint->joint_positions[4] += 0.1; // 若设备处于home位，则target就是当前点会规划轨迹为空，故加offset

        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() {
    auto& planner = GalbotMotion::get_instance(MachineType::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 = {
        {"torso",     {1.1}},
        {"head",      {0.0000, -0.26}},
        {"left_arm",  {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"torso", "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());
    }

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

    // --- 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.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.2267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.3267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.4267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}
        };

        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 = {
            {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.2267, 0.4342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.3267, 0.6342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.4267, 0.8342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}
        };

        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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);
    auto& navigation = GalbotNavigation::get_instance(MachineType::S1);

    // 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 = {
        {"torso",     {1.1}},
        {"head",      {0.0000, -0.26}},
        {"left_arm",  {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"torso", "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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 = "GE103v1";

        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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  auto& navigation = GalbotNavigation::get_instance(MachineType::S1);

  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 = {
        {"torso", {1.1}},
        {"head", {0.0000, -0.26}},
        {"left_arm", {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}},
    };
    std::vector<double> whole_body_joint;
    for (const auto& key : {"torso", "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 = {1.99995, -1.60004, 0.599905, -1.69994, 0.0, -0.799924, 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;
}

雅可比接口测试

适用场景：验证不同运动链、目标坐标系和指定机器人状态下的雅可比矩阵计算。

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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 (virtual chain, EndEffector maps to torso_base_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 a joint value (server treats leg the same as torso;
    //     S1 leg DOF = 1, so pass exactly one joint value) ---
    try {
        std::cout << "=== Test 14: leg chain / EndEffector / base_link (1 joint) ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> leg_joint_state = {{"leg", {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;
}

类：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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 double elapsed =
            std::chrono::duration<double>(std::chrono::steady_clock::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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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;
    }

    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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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(S1ControllerName::SWERVE_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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(S1ControllerName::SWERVE_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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(S1ControllerName::SWERVE_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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 double elapsed =
            std::chrono::duration<double>(std::chrono::steady_clock::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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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 obstacle 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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  // 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(S1ControllerName::SWERVE_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 elapsed_ms =
          std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::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 <chrono>
#include <iostream>
#include <thread>

#include "galbot_perception.hpp"
#include "galbot_robot.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::S1);
    robot.init();

    auto& perception = GalbotPerception::get_instance(MachineType::S1);
    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::S1);

    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/s1/cpp/tutorials 中；使用步骤与安全说明请参阅入门教程。

示例 1：Galbot 控制入门

适用场景：S1 最基础的关节控制：执行双臂比心动作并恢复初始姿态。运行前请确认机器人处于工作模式且急停按钮已旋开；夹爪控制及更多控制案例请参阅本页 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::S1); 

        /* 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, {"torso", "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"};
        // Heart pose: all angles verified within S1 joint limits (see robot_config.toml)
        std::vector<std::vector<double>> position_seq = {{
              1.00,  0.40, -2.06, -1.80, -2.92, -0.76, -0.03,// left_arm
             -1.00, -0.40,  2.06,  1.80,  2.92,  0.76,  0.03}}; // right_arm
        bool is_blocking = true;
        double max_speed = 0.3;
        double timeout_s = 30;

        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 <chrono>
#include <cctype>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <stdexcept>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

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

using namespace galbot::sdk;

void print_motion_status(MotionStatus status)
{
    if (status == MotionStatus::SUCCESS) {
        std::cout << "Execution result: SUCCESS, Execution successful\n";
    } else if (status == MotionStatus::TIMEOUT) {
        std::cout << "Execution result: TIMEOUT, Execution timeout\n";
    } else if (status == MotionStatus::FAULT) {
        std::cout << "Execution result: FAULT, Fault occurred, unable to continue execution\n";
    } else if (status == MotionStatus::INVALID_INPUT) {
        std::cout << "Execution result: INVALID_INPUT, Input parameters do not meet requirements\n";
    } else if (status == MotionStatus::INIT_FAILED) {
        std::cout << "Execution result: INIT_FAILED, Internal communication component creation failed\n";
    } else if (status == MotionStatus::IN_PROGRESS) {
        std::cout << "Execution result: IN_PROGRESS, Moving but not in position\n";
    } else if (status == MotionStatus::STOPPED_UNREACHED) {
        std::cout << "Execution result: STOPPED_UNREACHED, Stopped but did not reach target\n";
    } else if (status == MotionStatus::DATA_FETCH_FAILED) {
        std::cout << "Execution result: DATA_FETCH_FAILED, Data acquisition failed\n";
    } else if (status == MotionStatus::PUBLISH_FAIL) {
        std::cout << "Execution result: PUBLISH_FAIL, Data sending failed\n";
    } else if (status == MotionStatus::COMM_DISCONNECTED) {
        std::cout << "Execution result: COMM_DISCONNECTED, Connection failed\n";
    } else {
        std::cout << "Execution result: status=" << static_cast<int>(status) << "\n";
    }
}

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_val(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);

    double qw = clamp_val(q_err[3], -1.0, 1.0);

    return 2.0 * std::acos(qw);
}

struct PoseError
{
    double position_error_norm;
    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
    };
}

void print_pose_error(const char* label, const PoseError& e)
{
    std::cout << label
              << " position_error_norm: " << e.position_error_norm
              << " m, orientation_error_rad: " << e.orientation_error_rad
              << ", orientation_error_deg: " << e.orientation_error_deg << "\n";
}

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\n";

    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(static_cast<unsigned char>(key)) == 'y') {
            std::cout << "User confirmed, continuing execution...\n\n";
            break;
        }
        if (std::tolower(static_cast<unsigned char>(key)) == 'n') {
            std::cout << "User not confirmed, program exiting...\n\n";
            std::exit(1);
        }
        std::cout << "Input error, please enter 'y' or 'n'\n\n";
    }
}

void print_pose_line(const char* prefix, const std::vector<double>& pose)
{
    std::cout << prefix;
    for (double v : pose) {
        std::cout << v << " ";
    }
    std::cout << "\n";
}

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

        struct SdkCleanup {
            GalbotRobot& r;
            explicit SdkCleanup(GalbotRobot& ref) : r(ref) {}
            ~SdkCleanup()
            {
                r.request_shutdown();
                r.wait_for_shutdown();
                r.destroy();
            }
        } cleanup{robot};

        if (motion.init()) {
            std::cout << "GalbotMotion initialization successful\n";
        } else {
            std::cout << "GalbotMotion initialization failed\n";
        }
        if (robot.init()) {
            std::cout << "GalbotRobot initialization successful\n";
        } else {
            std::cout << "GalbotRobot initialization failed\n";
        }

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

        MotionStatus zero_status = motion.move_whole_body_joint_zero(
            true, 0.2, 15.0, std::make_shared<Parameter>());
        print_motion_status(zero_status);
        if (zero_status != MotionStatus::SUCCESS) {
            std::cout << "❌ move_whole_body_joint_zero failed, stop demo to avoid risky posture.\n";
            return 0;
        }
        std::cout << "✅ whole-body zero completed, start arm manipulation demo from safe pose.\n";

        const std::string target_frame = "EndEffector";
        const std::string reference_frame = "base_link";
        const std::string target_chain = "left_arm";
        const std::string end_link = "left_arm_end_effector_mount_link";

        std::vector<double> original_pose;
        {
            auto ret_pose = motion.get_end_effector_pose_on_chain(
                target_chain, target_frame, reference_frame);
            if (std::get<0>(ret_pose) != MotionStatus::SUCCESS) {
                std::cerr << "❌ Failed to get end-effector pose\n";
                return 0;
            }
            original_pose = std::get<1>(ret_pose);
            std::cout << "✅ Current " << target_chain << " end-effector pose: ";
            print_pose_line("", original_pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));

        std::vector<double> target_pose = {
            original_pose[0] + 0.04,
            original_pose[1],
            original_pose[2] + 0.02,
            original_pose[3],
            original_pose[4],
            original_pose[5],
            original_pose[6]
        };
        std::cout << "✅ S1 target pose (relative to zero pose): ";
        print_pose_line("", target_pose);

        std::unordered_map<std::string, std::vector<double>> joint_angles_ik;
        {
            const bool enable_collision_check = false;
            auto ret_ik = motion.inverse_kinematics(
                target_pose,
                {target_chain},
                target_frame,
                reference_frame,
                {},
                enable_collision_check,
                std::make_shared<Parameter>());
            if (std::get<0>(ret_ik) != MotionStatus::SUCCESS) {
                std::cerr << "❌ IK solving failed\n";
                return 0;
            }
            joint_angles_ik = std::get<1>(ret_ik);
            std::cout << "✅ Target " << target_chain << " IK solving successful joint_angles_ik: ";
            print_pose_line("", joint_angles_ik[target_chain]);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        ControlStatus set_joints_status = robot.set_joint_positions(
            joint_angles_ik[target_chain],
            {target_chain},
            {},
            true,
            0.1,
            20.0);
        if (set_joints_status != ControlStatus::SUCCESS) {
            std::cerr << "❌ Setting joint group angles failed\n";
            return 0;
        }
        std::cout << "✅ Setting " << target_chain << " joint group angles successful.\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        {
            auto ret_verify = motion.get_end_effector_pose_on_chain(
                target_chain, target_frame, reference_frame);
            if (std::get<0>(ret_verify) != MotionStatus::SUCCESS) {
                std::cerr << "❌ Failed to get end-effector pose after set joints\n";
                return 0;
            }
            const std::vector<double>& tgt_pose_ik = std::get<1>(ret_verify);
            std::cout << "✅ Getting " << target_chain << " end-effector pose successful: ";
            print_pose_line("", tgt_pose_ik);
            PoseError err = calculate_error(tgt_pose_ik, target_pose);
            print_pose_error("End-effector pose error:", err);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        {
            const bool enable_collision_check = false;
            auto ret_fk = motion.forward_kinematics(
                end_link,
                reference_frame,
                joint_angles_ik,
                std::make_shared<Parameter>());
            if (std::get<0>(ret_fk) != MotionStatus::SUCCESS) {
                std::cerr << "❌ FK solving failed\n";
                return 0;
            }
            const std::vector<double>& tgt_pose_fk = std::get<1>(ret_fk);
            std::cout << "✅ Target " << target_chain << " FK solving successful: ";
            print_pose_line("", tgt_pose_fk);
            PoseError err = calculate_error(tgt_pose_fk, target_pose);
            print_pose_error("FK solving error:", err);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        std::cout << "\n";

        {
            const bool enable_collision_check = false;
            MotionStatus ret_restore = motion.set_end_effector_pose(
                original_pose,
                target_chain,
                reference_frame,
                nullptr,
                enable_collision_check,
                true,
                5.0,
                std::make_shared<Parameter>());
            if (ret_restore != MotionStatus::SUCCESS) {
                std::cerr << "❌ Setting end-effector pose failed\n";
                return 0;
            }
            std::cout << "✅ Setting end-effector pose successful: status=" << static_cast<int>(ret_restore) << "\n";
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        {
            auto ret_final = motion.get_end_effector_pose_on_chain(
                target_chain, target_frame, reference_frame);
            if (std::get<0>(ret_final) != MotionStatus::SUCCESS) {
                std::cerr << "❌ Failed to get end-effector pose after restore\n";
                return 0;
            }
            const std::vector<double>& original_pose_rec = std::get<1>(ret_final);
            std::cout << "✅ Getting " << target_chain << " end-effector pose successful: ";
            print_pose_line("", original_pose_rec);
            PoseError err = calculate_error(original_pose_rec, original_pose);
            print_pose_error("Restore end-effector pose error:", err);
        }
    } catch (const std::exception& e) {
        std::cerr << "❌ Main program exception: " << e.what() << "\n";
        return 1;
    }

    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::S1); 
        auto& navi = GalbotNavigation::get_instance(MachineType::S1);        

        /* 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::S1); 
        
        /** 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::BACK_LIDAR,              /** BACK_LIDAR */
            SensorType::BACK_IMU              /** BACK_IMU*/
        };

        /* Initialize robot */
        if (robot.init(sensor_types)) {
            std::cout << "Initialization successful" << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
            return -1;
        }

        /* 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::BACK_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 back imu data */
        std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::BACK_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 输出转换为安全、协调的机器人关节指令。

examples/cpp/tutorials/src/example5_execute_vla.cpp

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

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

using namespace galbot::sdk;

namespace {
constexpr size_t kExpectedS1JointCount = 17;  // torso(1)+head(2)+left_arm(7)+right_arm(7)

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

std::vector<std::string> buildOrderedGroups(
    const std::map<std::string, std::vector<std::vector<double>>>& jointPositions) {
  std::vector<std::string> orderedGroups;
  for (const auto& group : kS1GroupOrder) {
    if (jointPositions.find(group) == jointPositions.end()) {
      throw std::runtime_error("Missing trajectory group: " + group);
    }
    orderedGroups.push_back(group);
  }
  return orderedGroups;
}

std::vector<std::vector<double>> mergeTrajectoryByGroupOrder(
    const std::map<std::string, std::vector<std::vector<double>>>& jointPositions,
    const std::vector<std::string>& orderedGroups) {
  if (orderedGroups.empty()) {
    return {};
  }

  const auto& firstGroupTraj = jointPositions.at(orderedGroups.front());
  if (firstGroupTraj.empty()) {
    return {};
  }

  std::vector<std::vector<double>> mergedTraj = firstGroupTraj;
  for (size_t groupIdx = 1; groupIdx < orderedGroups.size(); ++groupIdx) {
    const auto& groupTraj = jointPositions.at(orderedGroups[groupIdx]);
    if (groupTraj.size() != mergedTraj.size()) {
      throw std::runtime_error("Trajectory point count mismatch in group: " + orderedGroups[groupIdx]);
    }
    for (size_t pointIdx = 0; pointIdx < mergedTraj.size(); ++pointIdx) {
      mergedTraj[pointIdx].insert(mergedTraj[pointIdx].end(), groupTraj[pointIdx].begin(), groupTraj[pointIdx].end());
    }
  }
  return mergedTraj;
}

void printJointGroupSummary(const std::vector<std::string>& jointGroups, const std::vector<JointState>& jointStates) {
  std::cout << "Joint groups to execute: [";
  for (const auto& group : jointGroups) {
    std::cout << group << " ";
  }
  std::cout << "]" << std::endl;
  std::cout << "Joint states size before execution: " << jointStates.size() << std::endl;
}

double computeAbsPositionErrorNorm(const std::vector<double>& target, const std::vector<JointState>& measured) {
  const size_t n = std::min(target.size(), measured.size());
  double errorNorm = 0.0;
  for (size_t i = 0; i < n; ++i) {
    errorNorm += std::abs(target[i] - measured[i].position);
  }
  return errorNorm;
}
}  // namespace

/**
 * Generate target point for trajectory
 */
TrajectoryPoint generateTargetPoint(const std::vector<double>& q, double targetTime = 10.0) {
  TrajectoryPoint jointPosition;
  jointPosition.time_from_start_second = targetTime;

  for (double joint : q) {
    JointCommand jointCmd;
    jointCmd.position = joint;
    jointPosition.joint_command_vec.push_back(jointCmd);
  }

  return jointPosition;
}

/**
 * Generate trajectory for joints
 */
std::shared_ptr<Trajectory> generateTargetTrajectory(const std::vector<std::vector<double>>& trajectory,
                                                     const std::vector<std::string>& jointGroups,
                                                     const std::vector<std::string>& jointNames, double dt = 0.008) {
  if (trajectory.empty()) {
    return nullptr;
  }

  auto traj = std::make_shared<Trajectory>();
  traj->joint_groups = jointGroups;
  traj->joint_names = jointNames;

  double currentTime = 0.0;
  for (const auto& state : trajectory) {
    currentTime += dt;
    TrajectoryPoint trajPoint = generateTargetPoint(state, currentTime);
    traj->points.push_back(trajPoint);
  }

  return traj;
}

/**
 * Print joint states
 */
void printJointStates(const std::vector<JointState>& jointStates) {
  for (const auto& js : jointStates) {
    std::cout << " : position = " << js.position << " , velocity = " << js.velocity
              << " , acceleration = " << js.acceleration << " , effort = " << js.effort << " , current = " << js.current
              << std::endl;
  }
}

/**
 * Fake VLA (Vision-Language-Action) model implementation
 * Generate a small, safe whole-body trajectory based on S1 official zero pose
 */
std::map<std::string, std::vector<std::vector<double>>> fakeVla(
    const std::unordered_map<SensorType, std::shared_ptr<cv::Mat>>& rgbDataDict) {
  std::cout << "Fake VLA executing..." << std::endl;

  const int numPoints = 200;
  std::map<std::string, std::vector<std::vector<double>>> result;

  // S1 zero pose (kept consistent with GalbotMotionS1::move_whole_body_joint_zero)
  const std::vector<double> torso_zero{1.1};
  const std::vector<double> head_zero{0.0, -0.26};
  const std::vector<double> left_arm_zero{-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0};
  const std::vector<double> right_arm_zero{0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0};

  // Torso: Move linearly from 1.1 to 0.7
  std::vector<std::vector<double>> torsoTraj(numPoints, std::vector<double>(1));
  const double torso_start = 1.1;
  const double torso_end = 0.7;
  for (int i = 0; i < numPoints; ++i) {
    double s = static_cast<double>(i) / (numPoints - 1);  // 0 -> 1
    torsoTraj[i][0] = torso_start + (torso_end - torso_start) * s;
  }

  // Head: Apply small sinusoidal motion on the pitch joint
  std::vector<std::vector<double>> headTraj(numPoints, std::vector<double>(2));
  for (int i = 0; i < numPoints; ++i) {
    double alpha = std::sin(M_PI * i / (numPoints - 1));  // 0 -> 1 -> 0
    headTraj[i][0] = head_zero[0];
    headTraj[i][1] = head_zero[1] + 0.1 * alpha;  // Slightly oscillate around -0.26
  }

  // Arm: Start from zero pose and apply small linear motion on joint 4 for each arm
  auto makeArmTraj = [numPoints](const std::vector<double>& zero, const std::vector<double>& delta) {
    std::vector<std::vector<double>> traj(numPoints, std::vector<double>(zero.size()));
    for (int i = 0; i < numPoints; ++i) {
      double s = static_cast<double>(i) / (numPoints - 1);  // 0 -> 1
      for (size_t j = 0; j < zero.size(); ++j) {
        traj[i][j] = zero[j] + delta[j] * s;
      }
    }
    return traj;
  };

  std::vector<double> left_delta(left_arm_zero.size(), 0.0);
  std::vector<double> right_delta(right_arm_zero.size(), 0.0);
  // Example: make only small motions near the elbow joint; reduce the amplitude further for more conservative behavior
  if (left_delta.size() > 3) {
    left_delta[3] = 0.2;
  }
  if (right_delta.size() > 3) {
    right_delta[3] = -0.2;
  }

  std::vector<std::vector<double>> leftArmTraj = makeArmTraj(left_arm_zero, left_delta);
  std::vector<std::vector<double>> rightArmTraj = makeArmTraj(right_arm_zero, right_delta);

  result["torso"] = torsoTraj;
  result["head"] = headTraj;
  result["left_arm"] = leftArmTraj;
  result["right_arm"] = rightArmTraj;

  return result;
}

std::map<std::string, std::vector<std::vector<double>>> estimate_vla(
    GalbotRobot& robot, const std::unordered_set<SensorType>& enable_sensor_set) {
  /** Get RGB images from enabled cameras */
  std::unordered_map<SensorType, std::shared_ptr<cv::Mat>> rgb_images;
  for (const auto& sensor_type : enable_sensor_set) {
    if (sensor_type == SensorType::LEFT_ARM_CAMERA || sensor_type == SensorType::RIGHT_ARM_CAMERA) {
      std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(sensor_type, RgbOutputFormat::JPEG, true);
      if (rgb_data) {
        std::shared_ptr<cv::Mat> rgb_image = rgb_data->convert_to_cv2_mat();
        rgb_images[sensor_type] = rgb_image;
      } else {
        std::cerr << "Failed to get RGB data. " << std::endl;
      }
    }
  }
  std::cout << "RGB images size: " << rgb_images.size() << std::endl;

  /** Estimate VLA */
  std::map<std::string, std::vector<std::vector<double>>> vla = fakeVla(rgb_images);
  return vla;
}

/* @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::S1);
    auto& motion = GalbotMotion::get_instance(MachineType::S1);
    auto& navigation = GalbotNavigation::get_instance(MachineType::S1);

    /** Enable sensor type - S1 only has arm cameras, not head cameras */
    std::unordered_set<SensorType> enable_sensor_set = {SensorType::LEFT_ARM_CAMERA, SensorType::RIGHT_ARM_CAMERA};

    /* Initialize robot */
    if (robot.init(enable_sensor_set)) {
      std::cout << "Initialization successful" << std::endl;
      std::cout << "Is robot running: " << robot.is_running() << std::endl;
    } else {
      std::cerr << "Initialization failed" << std::endl;
    }
    if (!motion.init()) {
      std::cerr << "Motion initialization failed" << std::endl;
    } else {
      std::cout << "Motion initialization successful" << std::endl;
    }
    if (!navigation.init()) {
      std::cerr << "Navigation initialization failed" << std::endl;
    } else {
      std::cout << "Navigation initialization successful" << std::endl;
    }

    /* Wait for data preparation */
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    /** Estimate VLA actions */
    std::map<std::string, std::vector<std::vector<double>>> joint_positions = estimate_vla(robot, enable_sensor_set);

    // Generate target trajectory with fixed group order to avoid map-order ambiguity.
    std::vector<std::string> jointGroups = buildOrderedGroups(joint_positions);
    std::vector<std::vector<double>> jointTraj = mergeTrajectoryByGroupOrder(joint_positions, jointGroups);
    if (jointTraj.empty()) {
      throw std::runtime_error("Merged trajectory is empty.");
    }

    // Final joint position check
    std::vector<double> wholeBodyJoint = jointTraj.back();
    if (wholeBodyJoint.size() != kExpectedS1JointCount) {
      throw std::runtime_error("Invalid joint dimension: expected 17, got " + std::to_string(wholeBodyJoint.size()));
    }
    std::vector<double> baseState = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

    RobotStates checkState;
    checkState.whole_body_joint = wholeBodyJoint;
    checkState.base_state = baseState;

    std::cout << "✅ Final joint position check state: [";
    for (double val : wholeBodyJoint)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    // Check collision
    // std::tuple<MotionStatus, std::vector<bool>> collisionResult =
    // motion.check_collision(std::vector<std::shared_ptr<RobotStates>>{std::make_shared<RobotStates>(checkState)},
    // true); MotionStatus status = std::get<0>(collisionResult); std::vector<bool> collisionRes =
    // std::get<1>(collisionResult);
    auto [status, collisionRes] = motion.check_collision(
        std::vector<std::shared_ptr<RobotStates>>{std::make_shared<RobotStates>(checkState)}, true);
    std::this_thread::sleep_for(std::chrono::seconds(1));

    if (status == MotionStatus::SUCCESS) {
      std::cout << "✅ OK: collision check finished: " << collisionRes[0] << " (False=no collision)" << std::endl;

      auto beforeJointStates = robot.get_joint_states(jointGroups, {});
      printJointGroupSummary(jointGroups, beforeJointStates);
      printJointStates(beforeJointStates);
      std::cout << "Absolute position error norm before execution: "
                << computeAbsPositionErrorNorm(wholeBodyJoint, beforeJointStates) << std::endl;

      // Execute trajectory
      auto trajectory = generateTargetTrajectory(jointTraj, jointGroups, {});
      if (trajectory != nullptr) {
        ControlStatus execStatus = robot.execute_joint_trajectory(*trajectory, true);
        std::this_thread::sleep_for(std::chrono::seconds(1));

        if (execStatus == ControlStatus::SUCCESS) {
          std::cout << "✅ Joint trajectory execution successful." << std::endl;
        } else {
          std::cout << "❌ Joint trajectory execution failed." << std::endl;
          std::cout << "Diagnostic: target points = " << trajectory->points.size()
                    << ", joint dimension = " << wholeBodyJoint.size() << std::endl;
        }

        // Check joint state
        auto jointStates = robot.get_joint_states(jointGroups, {});
        printJointStates(jointStates);
        std::cout << "Absolute position error norm after execution: "
                  << computeAbsPositionErrorNorm(wholeBodyJoint, jointStates) << std::endl;
        std::cout << "✅ Final joint position check state after execution" << std::endl;
      } else {
        std::cout << "❌ Generated trajectory is invalid, cannot execute." << std::endl;
      }
    } else {
      std::cout << "❌ Collision check failed, will not execute the joint trajectory." << 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;
}

示例 6：综合任务：导航+感知+抓取+放置

适用场景：将导航、感知、抓取和放置组合为完整自主任务。

examples/cpp/tutorials/src/example6_pick_and_place.cpp

#include <array>
#include <chrono>
#include <cmath>
#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;

namespace {
// Demo mode: do not perform real chassis navigation to avoid on-site collision risk (only print simulated steps)
constexpr bool kDemoMockNavigationOnly = true;

// Resolve concrete names at runtime so both S1 V1 and S1 V2 are supported.
const std::vector<std::string> kS1BodyJointGroupsForSnapshot = {
    S1JointGroup::TORSO, S1JointGroup::HEAD, S1JointGroup::LEFT_ARM, S1JointGroup::RIGHT_ARM};
constexpr std::size_t kS1BodyJointCount = 17;

void mock_navigation_log(const char* step_description) {
  std::cout << "[Mock navigation] " << step_description << "(navigate_to_goal is not called, and the chassis does not move)" << std::endl;
}
}  // namespace

// 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;
  }

  // Create quaternion from rotation matrix
  static QuaternionST from_matrix(const std::array<std::array<double, 3>, 3>& mat) {
    double trace = mat[0][0] + mat[1][1] + mat[2][2];
    QuaternionST q;

    if (trace > 0) {
      double s = 0.5 / std::sqrt(trace + 1.0);
      q.w = 0.25 / s;
      q.x = (mat[2][1] - mat[1][2]) * s;
      q.y = (mat[0][2] - mat[2][0]) * s;
      q.z = (mat[1][0] - mat[0][1]) * s;
    } else if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
      double s = 2.0 * std::sqrt(1.0 + mat[0][0] - mat[1][1] - mat[2][2]);
      q.w = (mat[2][1] - mat[1][2]) / s;
      q.x = 0.25 * s;
      q.y = (mat[0][1] + mat[1][0]) / s;
      q.z = (mat[0][2] + mat[2][0]) / s;
    } else if (mat[1][1] > mat[2][2]) {
      double s = 2.0 * std::sqrt(1.0 + mat[1][1] - mat[0][0] - mat[2][2]);
      q.w = (mat[0][2] - mat[2][0]) / s;
      q.x = (mat[0][1] + mat[1][0]) / s;
      q.y = 0.25 * s;
      q.z = (mat[1][2] + mat[2][1]) / s;
    } else {
      double s = 2.0 * std::sqrt(1.0 + mat[2][2] - mat[0][0] - mat[1][1]);
      q.w = (mat[1][0] - mat[0][1]) / s;
      q.x = (mat[0][2] + mat[2][0]) / s;
      q.y = (mat[1][2] + mat[2][1]) / s;
      q.z = 0.25 * s;
    }

    return q;
  }
};

// 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];
  }

  // Matrix multiplication
  Transform operator*(const Transform& other) const {
    Transform result;
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        result.mat[i][j] = 0;
        for (int k = 0; k < 4; ++k) {
          result.mat[i][j] += mat[i][k] * other.mat[k][j];
        }
      }
    }
    return result;
  }

  // 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;
  }

  // Get rotation as quaternion
  QuaternionST get_rotation() const {
    std::array<std::array<double, 3>, 3> rot;
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        rot[i][j] = mat[i][j];
      }
    }
    return QuaternionST::from_matrix(rot);
  }

  // Get translation
  std::array<double, 3> get_translation() const { return {mat[0][3], mat[1][3], mat[2][3]}; }
};

cv::Mat decode_rgb_image(const std::vector<uint8_t>& image_data) {
  cv::Mat nparr(1, image_data.size(), CV_8UC1, (void*) image_data.data());
  cv::Mat img = cv::imdecode(nparr, cv::IMREAD_COLOR);

  if (img.empty()) {
    throw std::runtime_error("Failed to decode RGB Image");
  }
  return img;
}

cv::Mat decode_depth_image(const std::vector<uint8_t>& image_data, float depth_scale, int height = 720,
                           int width = 1280) {
  cv::Mat depth_img(height, width, CV_16UC1);
  std::memcpy(depth_img.data, image_data.data(), image_data.size());

  cv::Mat depth_img_float;
  depth_img.convertTo(depth_img_float, CV_32FC1, 1.0 / depth_scale);

  return depth_img_float;
}

std::vector<double> get_navigation_pose(const std::vector<double>& object_goal_pose, GalbotMotion& motion,
                                        const std::string& arm = "left_arm") {
  if (arm != "left_arm" && arm != "right_arm") {
    throw std::invalid_argument("arm must be left_arm or right_arm");
  }

  try {
    // FIXME:EXAMPLE[8]: Verify S1 end effector link name
    auto [status, ee_pose_in_base] =
        motion.get_end_effector_pose(arm + "_end_effector_mount_link",  // FIXME[8]: verify link name for S1
                                     "base_link");

    double offset_y;
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to get end effector pose: status=" << (int) status << std::endl;
      offset_y = 0.0;  // Use default if failed
    } else {
      std::cout << "✅ Successfully got end effector pose" << std::endl;
      offset_y = 0.3;
    }

    // Create transformation matrix for base goal pose
    Transform base_goal_pose_mat;
    QuaternionST obj_quat(object_goal_pose[3], object_goal_pose[4], object_goal_pose[5], object_goal_pose[6]);
    base_goal_pose_mat.set_rotation(obj_quat);
    base_goal_pose_mat.set_translation({object_goal_pose[0], object_goal_pose[1], 0.0});

    // Apply offset: move 0.6m backward and offset_y in y direction
    Transform offset_mat;
    offset_mat.set_translation({-0.6, -offset_y, 0.0});

    base_goal_pose_mat = base_goal_pose_mat * offset_mat;

    // Extract pose
    auto pos = base_goal_pose_mat.get_translation();
    auto quat = base_goal_pose_mat.get_rotation();

    return {pos[0], pos[1], pos[2], quat.x, quat.y, quat.z, quat.w};

  } catch (const std::exception& e) {
    std::cout << "Failed to get navigation target pose: " << e.what() << std::endl;
    return {1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0};
  }
}

// Real navigation (not called in the demo flow by default to avoid chassis motion)
void navigation_to_goal(GalbotNavigation& nav, const std::vector<double>& goal_pose, int retry_cnt = 3) {
  if (kDemoMockNavigationOnly) {
    mock_navigation_log("navigate_to_goal skipped");
    (void) nav;
    (void) goal_pose;
    (void) retry_cnt;
    return;
  }
  try {
    auto cur_pose = nav.get_current_pose();
    std::cout << "Current pose: [";
    std::cout << "]" << std::endl;

    if (nav.check_path_reachability(goal_pose, cur_pose)) {
      retry_cnt = 3;
      NavigationStatus status;

      while (true) {
        status = nav.navigate_to_goal(goal_pose, true, true, 20.0);
        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;
  }
}

// GalbotMotionS1::move_whole_body_joint_zero: joint, base end effector z
// Move downward slightly (for camera to look down at table). downward_delta_m: distance to move down from current end-effector height (meters); adjust smaller/larger on-site as needed.
void lower_camera_after_joint_zero(GalbotMotion& motion, const std::string& target_chain,
                                   const std::string& reference_frame, double downward_delta_m = 0.08) {
  try {
    std::cout << "[Camera pose] First run move_whole_body_joint_zero (S1 zero pose consistent with SDK), then move downward relative to end effector by Δz="
              << downward_delta_m << " m" << std::endl;

    auto zero_status = motion.move_whole_body_joint_zero(true, 0.2, 15.0, std::make_shared<Parameter>());
    if (zero_status != MotionStatus::SUCCESS) {
      std::cout << "⚠️ move_whole_body_joint_zero not successful: status=" << static_cast<int>(zero_status)
                << ", still trying to read the end and fine-tune …" << std::endl;
    } else {
      std::cout << "✅ move_whole_body_joint_zero completed successfully" << std::endl;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(800));

    int retry_cnt = 3;
    std::vector<double> cur_ee_pose;
    MotionStatus status;

    while (true) {
      std::tie(status, cur_ee_pose) =
          motion.get_end_effector_pose_on_chain(target_chain, "EndEffector", reference_frame);

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

      if (status == MotionStatus::SUCCESS) {
        std::cout << "✅ Successfully got end effector pose (after zero)" << std::endl;
        break;
      }
      if (retry_cnt < 0) {
        cur_ee_pose = {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991};
        std::cout << "❌ Failed to get end effector pose, using default" << std::endl;
        break;
      }
      std::cout << "Failed to get end effector pose: status=" << static_cast<int>(status) << ", retrying: " << retry_cnt
                << std::endl;
    }

    std::vector<double> tgt_ee_pose = cur_ee_pose;
    // base_link: +z is upward; from top-down view, decrease z to move downward
    tgt_ee_pose[2] -= downward_delta_m;
    // Avoid lowering too much at once (can be adjusted according to the on-site workbench height)
    constexpr double kMinEeZBaseLink = 0.15;
    if (tgt_ee_pose[2] < kMinEeZBaseLink) {
      std::cout << "⚠️ Target z=" << tgt_ee_pose[2] << " Below the safety lower limit " << kMinEeZBaseLink << ", clamped" << std::endl;
      tgt_ee_pose[2] = kMinEeZBaseLink;
    }

    std::cout << "Target end effector pose (after downward fine-tuning): [";
    for (auto val : tgt_ee_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    retry_cnt = 3;
    while (true) {
      status = motion.set_end_effector_pose(tgt_ee_pose, target_chain, reference_frame, nullptr, false, true, 5.0,
                                            std::make_shared<Parameter>());

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

      if (status == MotionStatus::SUCCESS) {
        std::cout << "✅ Successfully set end effector pose: status=" << static_cast<int>(status) << std::endl;
        break;
      }
      if (retry_cnt < 0) {
        std::cout << "❌ Failed to set end effector pose after retries: status=" << static_cast<int>(status)
                  << std::endl;
        break;
      }
      std::cout << "Failed to set end effector pose: status=" << static_cast<int>(status) << ", retrying: " << retry_cnt
                << std::endl;
    }
  } catch (const std::exception& e) {
    std::cout << "❌ Exception in lower_camera_after_joint_zero: " << e.what() << std::endl;
  }
}

std::vector<double> detect_target(const cv::Mat& img, const cv::Mat& depth_img) {
  try {
    // NOTE: This is a placeholder function
    // In real-world scenario, implement actual computer vision detection

    // OpenCV frame: x right, y down, z forward
    std::vector<double> default_pose = {0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71};

    return default_pose;
  } catch (const std::exception& e) {
    std::cout << "Target detection exception: " << e.what() << std::endl;
    return {};
  }
}

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::string& arm = "left_arm") {
  std::vector<double> object_pose_base;

  try {
    SensorType rgb_sensor, depth_sensor;

    if (arm == "left_arm") {
      rgb_sensor = SensorType::LEFT_ARM_CAMERA;
      depth_sensor = SensorType::LEFT_ARM_DEPTH_CAMERA;
    } else if (arm == "right_arm") {
      rgb_sensor = SensorType::RIGHT_ARM_CAMERA;
      depth_sensor = SensorType::RIGHT_ARM_DEPTH_CAMERA;
    } else {
      throw std::invalid_argument("arm must be left_arm or right_arm");
    }

    // Get RGB image data
    auto rgb_image_data = robot.get_rgb_data(rgb_sensor, RgbOutputFormat::JPEG, true);
    auto depth_data = robot.get_depth_data(depth_sensor);

    cv::Mat img, depth_img;

    if (rgb_image_data && !rgb_image_data->data.empty()) {
      std::cout << "Get rgb image success" << std::endl;
      auto mat_ptr = rgb_image_data->convert_to_cv2_mat();
      if (mat_ptr && !mat_ptr->empty()) {
        img = *mat_ptr;
      } else {
        img = decode_rgb_image(rgb_image_data->data);
      }
    } else {
      std::cout << "No rgb image data (sensor may be unavailable)" << std::endl;
    }

    if (depth_data && depth_data->width > 0 && depth_data->height > 0 && !depth_data->data.empty()) {
      std::cout << "Get depth data success" << std::endl;
    } else {
      std::cout << "No depth data (depth sensor may be unavailable)" << std::endl;
    }

    // Detect target (placeholder)
    auto object_pose_camera = detect_target(img, depth_img);

    if (object_pose_camera.empty()) {
      std::cout << "Target detection failed" << std::endl;
      return {};
    } else {
      std::cout << "object_pose_camera: [";
      for (auto val : object_pose_camera)
        std::cout << val << " ";
      std::cout << "]" << std::endl;
    }

    // Transform to base frame
    object_pose_base = pose_camera_to_base(robot, object_pose_camera);
    std::cout << "Target pose in chassis coordinate system: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

  } catch (const std::exception& e) {
    object_pose_base = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
    std::cout << "Target detection exception: " << e.what() << std::endl;
  }

  return object_pose_base;
}

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;
    }
  }
}

void pick_and_place(GalbotRobot& robot, GalbotMotion& motion, const std::vector<double>& object_pose_base,
                    const std::string& target_chain, const std::string& reference_frame) {
  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;
    } else {
      std::cout << "✅ Successfully attached tool: status=" << (int) status << std::endl;
    }

    // Open left gripper
    auto gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, true);
    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;

    std::cout << "object_pose_base: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    // Reach to target position
    int retry_cnt = 3;
    while (true) {
      status = motion.set_end_effector_pose(object_pose_base, target_chain, reference_frame, nullptr, false, true, 5.0,
                                            std::make_shared<Parameter>());

      std::this_thread::sleep_for(std::chrono::seconds(1));
      retry_cnt--;

      if (status == MotionStatus::SUCCESS || retry_cnt < 0) {
        break;
      } else {
        std::cout << "Failed to set end effector pose: status=" << (int) status << ", retry count: " << retry_cnt
                  << std::endl;
      }
    }

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to set end effector pose: status=" << (int) status << std::endl;
    } else {
      std::cout << "✅ Successfully set end effector pose: status=" << (int) status << std::endl;
    }

    // Close gripper to grasp object
    gripper_status = robot.set_gripper_command("left_gripper", 0.02, 0.05, 10, true);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully closed left gripper: status=" << (int) gripper_status << std::endl;

    // Attach target object (box scale values must all be >0; default {} gives 0 and causes INVALID_INPUT)
    std::array<double, 3> grasp_box_scale{0.05, 0.05, 0.05};
    status = motion.attach_target_object("grasped_box", "box", {0, 0, 0, 0, 0, 0, 1}, grasp_box_scale, "", "left_arm",
                                         "ee_base");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to attach target object: status=" << (int) status << std::endl;
    } else {
      std::cout << "✅ Successfully attached target object: status=" << (int) status << std::endl;
    }

    // Return to the initial position: in demo mode, only simulate and do not call chassis navigation
    mock_navigation_log("After grasping, return to the starting point");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ (Mock) Return navigation skipped; chassis not moved" << std::endl;

    // Release target
    gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, true);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully released left gripper: status=" << (int) gripper_status << std::endl;

    // Detach target object
    status = motion.detach_target_object("grasped_box");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to detach target object: status=" << (int) status << std::endl;
    } else {
      std::cout << "✅ Successfully detached target object: status=" << (int) 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;
    }

  } catch (const std::exception& e) {
    std::cout << "Exception occurred during pick_and_place: " << e.what() << std::endl;
  }
}

int main() {
  check_robot_safety();

  // Get robot instances
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  auto& motion = GalbotMotion::get_instance(MachineType::S1);
  auto& nav = GalbotNavigation::get_instance(MachineType::S1);

  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 (kDemoMockNavigationOnly) {
      std::cout << "[Demo mode] Skipped: relocalize and real navigate_to_goal (chassis remains stationary)." << std::endl;
    } else {
      while (true) {
        if (nav.is_localized()) {
          std::cout << "✅ Robot localized successfully, proceeding to navigate to goal..." << std::endl;
          break;
        }
        std::cout << "❌ Navigation not localized, relocalizing..." << std::endl;
        nav.relocalize(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::seconds(2));
      }
    }

    mock_navigation_log("Before starting the task: move near the target point");

    // Save the current body joints using the hardware-version-specific names exposed by the SDK.
    const auto snapshot_joint_names = robot.get_joint_names(true, kS1BodyJointGroupsForSnapshot);
    if (snapshot_joint_names.size() != kS1BodyJointCount) {
      throw std::runtime_error("Failed to resolve complete S1 body joint names for snapshot: got " +
                               std::to_string(snapshot_joint_names.size()) + ", expected " +
                               std::to_string(kS1BodyJointCount));
    }
    auto joint_groups_positions = robot.get_joint_positions({}, snapshot_joint_names);
    std::cout << "Saved body joint snapshot: " << joint_groups_positions.size() << " values (expected "
              << snapshot_joint_names.size() << ")" << std::endl
              << std::endl;
    if (joint_groups_positions.size() != snapshot_joint_names.size()) {
      throw std::runtime_error("Failed to save complete body joint snapshot: got " +
                               std::to_string(joint_groups_positions.size()) + " values for " +
                               std::to_string(snapshot_joint_names.size()) + " joints");
    }

    // joint, end effector (lower_camera_after_joint_zero)
    lower_camera_after_joint_zero(motion, "left_arm", "base_link", 0.08);
    std::cout << std::endl;

    // Detect target
    auto object_pose_base = detect_object(robot, "left_arm");
    std::cout << std::endl;

    // Pick and place
    pick_and_place(robot, motion, object_pose_base, "left_arm", "base_link");
    std::cout << std::endl;

    // Restore joint positions
    int retry_cnt = 3;
    while (true) {
      auto status = robot.set_joint_positions(joint_groups_positions, {}, snapshot_joint_names, true, 0.5, 20);
      std::this_thread::sleep_for(std::chrono::milliseconds(500));

      if (status == ControlStatus::SUCCESS) {
        std::cout << "✅ Successfully set joint positions: status=" << (int) status << std::endl;
        break;
      } else if (retry_cnt < 0) {
        std::cout << "❌ Failed to set joint positions after retries, status=" << (int) status << std::endl;
        break;
      } else {
        retry_cnt--;
        std::cout << "Failed to set joint positions: retry count: " << retry_cnt << 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;
}