### Complete Running Example (Simple Flow)

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/zh/examples_cpp/index.html

This snippet demonstrates a complete navigation flow, including checking path reachability, navigating to a goal, and returning to the start point. It also shows how to handle navigation failures and stop the robot.

```cpp
#include <iostream>
#include <thread>
#include <chrono>

#include "galbot/navigation.h"
#include "galbot/robot.h"

int main() {
    // Initialize robot and navigation system
    galbot::Robot robot;
    galbot::Navigation navigation;

    // Initialize robot and navigation system
    if (robot.init() && navigation.init()) {
        // Define initial and goal poses (example values)
        Pose init_pose;
        init_pose.x = 0.0; init_pose.y = 0.0; init_pose.theta = 0.0;
        Pose goal_pose;
        goal_pose.x = 1.0; goal_pose.y = 1.0; goal_pose.theta = 1.57;

        bool enable_collision_check = false;
        bool is_blocking = false;
        int timeout_s = 10;

        // Check path reachability and navigate to goal
        if (navigation.check_path_reachability(init_pose, goal_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 for arrival
            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 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;
}
```

--------------------------------

### Complete Running Example: Simple Workflow

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/examples_cpp/index.html

This snippet demonstrates a complete workflow including navigation to a goal, checking path reachability, returning to the start, and stopping navigation. It also includes relocalization and robot shutdown procedures.

```cpp
std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
            }
        }
        else {
            std::cout << "Path unreachable, cannot navigate to target point"
        }
        
        // 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"
            // 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::this_thread::sleep_for(std::chrono::milliseconds(1000));
                if (++count_arrival > 10) {
                    break;
                }
            }
            if (navigation.check_goal_arrival()) {
                std::cout << "Target point reached"
            }
            else {
                std::cout << "Navigation failed; target point not reached"
            }
        }
        else {
            std::cout << "Path unreachable, cannot navigate to start point"
        }
        
        // checkpath navigation target
        if (navigation.check_path_reachability(goal_pose, init_pose)) {
            std::cout << "Path reachable, navigating to target point"
            // 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"
            }
            else {
                std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
            }
        }
        else {
            std::cout << "Path unreachable, cannot navigate to target point"
        }
        
        // Stop navigation
        navigation.stop_navigation();
    }
    else {
        std::cout << "Relocalization failed!"
    }
    
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    
    return 0;
```

--------------------------------

### Install Galbot SDK on PC

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/installation_robot_deployment/index.html

Navigate to the SDK directory on your PC and run the installation script.

```bash
cd GalbotSDK
./install.sh
```

--------------------------------

### Start Local Documentation Server

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/README.md

Navigate to the docs directory and run this command to start a local HTTP server for viewing documentation. Open http://localhost:8000/zh/ for Chinese or http://localhost:8000/en/ for English.

```bash
cd docs
python3 -m http.server 8000
```

--------------------------------

### Get Gripper State in C++

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/zh/examples_cpp/index.html

This C++ example shows how to initialize the robot system, retrieve the state of a specified gripper (e.g., 'left_gripper'), and print its details. It includes error handling for initialization and state retrieval.

```cpp
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_gripper_state(std::shared_ptr<GripperState> gripper_state) {
    std::cout << "Timestamp (ns): " << gripper_state->timestamp_ns << std::endl;
    
    std::cout << " width "  << gripper_state->width << " velocity " << gripper_state->velocity
                << " effort " << gripper_state->effort << " is moving "
                << gripper_state->is_moving << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    
    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }
    
    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    
    // Get gripper state
    auto gripper_state_ptr = robot.get_gripper_state("left_gripper");
    
    if (gripper_state_ptr == nullptr) {
        std::cerr << "get gripper state error" << std::endl;
    } else {
        std::cout << "Left gripper state:" << std::endl;
        print_gripper_state(gripper_state_ptr);
    }
    
    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}
```

--------------------------------

### Start Mapping Server

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/routine_operations/index.html

Execute this command to start the mapping program. Ensure the robot's emergency stop is engaged and the robot is at the starting point.

```bash
/data/galbot/bin/mapping_server
```

--------------------------------

### Complete Running Example for Navigation

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/zh/examples_cpp/index.html

This example showcases a full navigation workflow, including initialization, relocalization, and moving to a target. It's useful for understanding the basic sequence of operations for robot navigation.

```cpp
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"
#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

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

    // Initialize system
    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << "Relocalization successful!" << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        // Whether to enable obstacle checking (can be set to true in open environments)
        bool enable_collision_check = false;
        // Whether to block and wait for arrival
        bool is_blocking = true;
        // Maximum wait time to reach position
        float timeout_s = 20;

        // checkpath navigation target
        if (navigation.check_path_reachability(goal_pose, init_pose)) {
            std::cout << "Path reachable, navigating to target point" << std::endl;
            // navigation, obstaclecheck, wait, wait 20
            NavigationStatus status = navigation.navigate_to_goal(
                goal_pose, enable_collision_check, is_blocking, timeout_s);
            if (status == NavigationStatus::SUCCESS) {
                std::cout << "Target point reached" << std::endl;
            } else {
                std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
            }
        }
    }
    return 0;
}
```

--------------------------------

### Get Gripper State Example

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/examples_cpp/index.html

This C++ snippet demonstrates how to initialize the Galbot Robot SDK, retrieve the state of the left gripper, and print its details. It includes error handling for initialization and state retrieval.

```cpp
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_gripper_state(std::shared_ptr<GripperState> gripper_state) {
    std::cout << "Timestamp (ns): " << gripper_state->timestamp_ns << std::endl;
    
    std::cout << " width "  << gripper_state->width << " velocity " << gripper_state->velocity
                << " effort " << gripper_state->effort << " is moving "
                << gripper_state->is_moving << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G1);
    
    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }
    
    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    
    // Get gripper state
    auto gripper_state_ptr = robot.get_gripper_state("left_gripper");
    
    if (gripper_state_ptr == nullptr) {
        std::cerr << "get gripper state error" << std::endl;
    } else {
        std::cout << "Left gripper state:" << std::endl;
        print_gripper_state(gripper_state_ptr);
    }
    
    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    
    return 0;
}
```

--------------------------------

### Example Output of 'q' Command

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/zh/troubleshooting/index.html

Example output from the 'q' command showing the current mode type. 'WORKING_MODE' indicates normal operation. Other modes may require further troubleshooting.

```text
current mode: mode_type: WORKING_MODE mode_switch_state {
  status: SUCCESS
}
```

--------------------------------

### Example sys_tool Mode Query Output

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/en/troubleshooting/index.html

Example output from the 'q' command in sys_tool, showing the current mode type and status.

```text
current mode: 
mode_type: WORKING_MODE 
mode_switch_state { 
  status: SUCCESS 
}
```

--------------------------------

### Get Joint States Example

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/zh/examples_cpp/index.html

This C++ example demonstrates how to retrieve and print the states of robot joints. It shows how to initialize the robot, fetch joint states by group names, and by specific joint names. Ensure the SDK is initialized before calling these functions.

```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::G1);
    
    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }
    
    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    
    // Get joint states by joint group names; returns all joints if empty
    std::vector<std::string> joint_groups = {"left_arm"};
    auto ret_states = robot.get_joint_states(joint_groups, {});
    
    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);
    
    print_joint_states(ret_states);
    
    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}
```

--------------------------------

### Initialize Robot and Sensors for Camera Data

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/examples_cpp/index.html

This main function example shows how to get the GalbotRobot instance, initialize it with specific sensor types (head left camera and left arm depth camera), and prints a success message upon successful initialization.

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

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

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

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    }
```

--------------------------------

### Get Joint States Example in C++

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/en/examples_cpp/index.html

Demonstrates how to initialize the Galbot SDK, retrieve joint states for specified groups or individual joints, and print the information. This is useful for algorithms requiring detailed joint data.

```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, {});
    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);
    print_joint_states(ret_states);
    
    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    
    return 0;
}
```

--------------------------------

### Install Python Dependencies

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/installation_pc_ubuntu/index.html

Install required Python libraries by running the provided shell script. Navigate to the GalbotSDK directory before execution.

```bash
cd GalbotSDK
./install_python_deps.sh
```

--------------------------------

### Start sys_tool on XCU

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/troubleshooting/index.html

Navigate to the /data/bin directory and start the sys_tool utility on the XCU.

```bash
cd /data/bin
./sys_tool
```

--------------------------------

### Verify SDK Installation on Robot

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/installation_robot_deployment/index.html

Connect to the robot via SSH and list the contents of the dynamic library directory to confirm the SDK installation.

```bash
ssh galbot@<Robot IP>
ls /data/galbot/lib | grep galbot_sdk
```

--------------------------------

### Start GalbotSDK Docker Container

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/installation_docker_deployment/index.html

Navigates to the docker directory within the SDK deliverable and executes the run script to start the GalbotSDK container. This script ensures all necessary SDK libraries and the runtime environment are available inside the container.

```bash
cd GalbotSDK/docker
./run.sh
```

--------------------------------

### C++ Example: Set Joint Commands in Batch Mode

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/en/examples_cpp/index.html

This C++ example demonstrates how to initialize the Galbot Robot SDK and send a batch of joint commands. It includes a helper function to generate trajectory data for a specified number of points and joints.

```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 = {};

  // ... (rest of the code would follow to generate trajectory and call set_joint_commands_batch)
  return 0;
}

```

--------------------------------

### Get Joint Positions Example

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/zh/examples_cpp/index.html

This example shows how to get joint positions for a specific joint group or individual joints. It initializes the robot system, retrieves positions, and prints them to the console. Use this when only joint positions are needed.

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

--------------------------------

### Print Menu C++ Example

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/examples_cpp/index.html

Prints a list of available commands to the console. This function is typically used in interactive command-line examples to guide the user.

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

--------------------------------

### Complete Navigation Example

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/zh/examples_cpp/index.html

Demonstrates the full navigation workflow, including initialization, relocalization, and reaching a goal pose. Use this as a reference for a typical navigation task.

```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;
            }
        }
    }
    return 0;
}

```

--------------------------------

### Get Joint Names

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/en/examples_python/index.html

Retrieves a list of all joint names for the robot, which can be used for iteration or configuration file generation. This example shows how to get names for a specific joint group.

```python
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get joint positions by joint group names; returns all joints if empty
joint_group_names = ["left_arm"]
# get joint
only_active_joint = True
ret = robot.get_joint_names(only_active_joint, joint_group_names)
print("Left joint names:")
for i, name in enumerate(ret):
    print(f"{i + 1}: {name}")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
```

--------------------------------

### Get Navigation Status and Poll for Completion

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/examples_cpp/index.html

This example demonstrates how to get the current navigation task status and poll for SUCCESS, FAILED, or timeout conditions. It's useful for non-blocking navigation and handling errors promptly.

```cpp
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

using namespace galbot::sdk;

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

    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G1ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    Pose goal_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    const double timeout_s = 20.0;
    const double poll_interval_s = 0.5;

    // Non-blocking navigation
    navigation.navigate_to_goal(goal_pose, true, false, static_cast<float>(timeout_s));
    auto start = std::chrono::steady_clock::now();

    while (true) {
        NavigationTaskStatus status = navigation.get_navigation_status();
        auto elapsed = std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count();

        if (status == NavigationTaskStatus::SUCCESS) {
            std::cout << "Target reached" << std::endl;
            break;
        }
        if (status == NavigationTaskStatus::FAILED) {
            std::cout << "Navigation failed; exit error-handling logic promptly" << std::endl;
            break;
        }
        if (elapsed >= timeout_s) {
            std::cout << "navigationtimeout, exit" << std::endl;
            break;
        }
        if (status == NavigationTaskStatus::RUNNING) {
            std::cout << "Navigating... Status: RUNNING, elapsed: " << elapsed << "s" << std::endl;
        } else {
            std::cout << "Status: UNKNOWN, : " << elapsed << "s" << std::endl;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(poll_interval_s * 1000)));
    }

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

--------------------------------

### Complete Navigation Example

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/zh/examples_python/index.html

Demonstrates a full navigation workflow, including initialization, relocalization, and navigating to a goal. Use this to understand the basic sequence of operations for navigation tasks.

```python
from galbot_sdk.g1 import GalbotRobot, GalbotNavigation
from galbot_sdk.g1 import ControlStatus, G1ControllerName
import numpy as np
import time
import sys

robot = GalbotRobot()
robot.init()

nav = GalbotNavigation()
nav.init()

init_pose = np.array([0.0, 0.0, 0.0, 0, 0, 0.0, 1.0])
goal_pose = np.array([1.0, 0.0, 0.0, 0, 0, 0.0, 1.0])

res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
    print("Failed to switch controller!")
    sys.exit(1)
else:
    print("Controller switched successfully!")

while not nav.is_localized():
    nav.relocalize(init_pose)
    time.sleep(0.5)

if nav.check_path_reachability(goal_pose, nav.get_current_pose()):
    nav.navigate_to_goal(
        goal_pose, enable_collision_check=True, is_blocking=True, timeout=30
    )
    print("Whether reached:", nav.check_goal_arrival())

nav.stop_navigation()
# Shutdown system
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
```

--------------------------------

### Get Joint Positions C++ Example

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/zh/examples_cpp/index.html

Demonstrates how to get joint positions for specific joint groups or individual joints. This is a lightweight alternative to get_joint_states when only position data is needed. Ensure the system is initialized before calling.

```cpp
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include "galbot_robot.hpp"

using namespace galbot::sdk;

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

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

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

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

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

    std::cout << "Left arm joint positions:" << std::endl;
    print_joint_positions(ret_positions);

    // Get specified joint positions; if provided, overrides joint group input
    std::vector<std::string> joint_names = {"left_arm_joint1", "left_arm_joint2"};
    ret_positions = robot.get_joint_positions({joint_groups}, joint_names);

    std::cout << "Positions of left arm joint1 and joint2:" << std::endl;
    print_joint_positions(ret_positions);

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

    return 0;
}
```

--------------------------------

### Initialize and Navigate with GalbotNavigation

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/api_cpp_reference/index.html

Demonstrates typical usage of the GalbotNavigation singleton. It initializes the navigation system, sets a goal, and initiates navigation.

```cpp
auto& nav = GalbotNavigation::get_instance(MachineType::G1);
if (nav.init()) {
    Pose goal;
    goal.x = 1.0; // meters
    goal.y = 2.0; // meters
    goal.orientation.w = 1.0; // identity quaternion (x,y,z default 0)
    nav.navigate_to_goal(goal, true, false, 30.0, true);
}
```

--------------------------------

### Move Robot to Original Start Pose

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/tutorials/index.html

Guides the robot back to its initial starting pose [0, 0, 0, 0, 0, 0, 1]. It checks for path reachability and implements a retry loop for navigation.

```python
cur_pose = nav.get_current_pose()
    
    goal_pose = [0, 0, 0, 0, 0, 0, 1] 

    try:
        if nav.check_path_reachability(goal_pose, cur_pose):
            retry_cnt = 3            
            while True:
                status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=30)
                time.sleep(0.5)
                retry_cnt -= 1
                if nav.check_goal_arrival() or retry_cnt < 0:
                    break
                else:
                    print(f"Navigation failed, retrying...{retry_cnt}")
            print("navigate_to_goal return status:", status)
            print("Has arrived:", nav.check_goal_arrival())
        else:
            print("Path unreachable or unsafe")
    except Exception as e:
        print(f"Exception occurred during navigation: {e}")
```

--------------------------------

### Display Command Menu

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/examples_python/index.html

Prints a menu of available commands to the console, guiding the user on how to interact with the robot control example.

```python
def print_menu():
    print(
        "\nAvailable commands:\n"
        "  joint        - request a joint-only head target\n"
        "  base_pose    - request a chassis pose target\n"
        "  base_twist   - request a chassis twist target with auto stop\n"
        "  mixed_pose   - request head + left_arm + chassis pose in one target\n"
        "  mixed_twist  - request head + left_arm + chassis twist in one target\n"
        "  quit         - exit example\n"
    )
```

--------------------------------

### Main Robot Initialization and Navigation Setup

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/zh/tutorials/index.html

Initializes the robot and navigation systems, then waits for data preparation. Includes a safety check before initialization.

```python
def main():
    check_robot_safety()
    try:
        # Get robot instance
        robot = GalbotRobot()
        # Get navigation instance
        nav = GalbotNavigation()

        # Initialize robot
        if robot.init():
            print("Robot initialization successful")
        else:
            print("Robot initialization failed")
        # Initialize navigation
        if nav.init():
            print("Navigation initialization successful")
        else:
            print("Navigation initialization failed")

        # Wait for data preparation
        time.sleep(1)
```

--------------------------------

### Get Camera Intrinsics and Sensor Extrinsics

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/s1/en/examples_python/index.html

Retrieves camera intrinsics and sensor extrinsics relative to the robot base. This is essential for computer vision tasks and 3D reconstruction. Ensure the galbot_sdk is installed and in your PYTHONPATH. OpenCV and NumPy are required and will be installed if not found.

```python
try:
    from galbot_sdk.s1 import GalbotRobot, SensorType
except ImportError:
    print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
    exit(1)

import os

try:
    import cv2
except ImportError:
    os.system("pip install opencv-python")
    import cv2

try:
    import numpy as np
except ImportError:
    os.system("pip install numpy")
    import numpy as np

import time
from typing import Dict

def decode_compressed_image(compressed_image):
    """
    decode CompressedImage image

    Parameters:
        compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]

    Returns:
        numpy.ndarray: decoded image
    """
    image_data = compressed_image["data"]
    if compressed_image["format"] == "rgb8":
        return decode_rgb_image(image_data)
    elif compressed_image["format"] == "16UC1":
        return decode_depth_image(compressed_image)
    else:
        raise ValueError(f"Unsupport data format: {compressed_image['format']}")

def decode_rgb_image(image_data):
    "decode rgb image"    nparr = np.frombuffer(image_data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    if img is None:
        raise ValueError("Fail to Decode RGB Image")
    return img

def decode_depth_image(image_data):
    "decode depth image"    depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
    
    # Check whether height and width exist
    if "height" not in image_data or "width" not in image_data:
        raise ValueError("Missing 'height' or 'width' in depth image metadata.")
    if image_data["height"] == 0 or image_data["width"] == 0:
        raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")
    
    # Parse depth image
    depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
    depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
    
    return depth_img

def main():
    robot = GalbotRobot()
    
    # Get left arm RGB and depth images, right arm depth image, chassis lidar data, torso IMU data
    enable_sensor_set = {
        SensorType.LEFT_ARM_CAMERA, # Left arm depth camera
        SensorType.LEFT_ARM_DEPTH_CAMERA, # Left arm RGB camera
    }
    robot.init(enable_sensor_set)
    print("Initialization succeeded")
    
    # Program started, waiting for data
    time.sleep(5)
    
    # Get left arm RGB image
    rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA)
    if not rgb_image_data:
        print("No rgb image data!")
    else:
        
```

--------------------------------

### Configure and Use SDK Logger

Source: https://github.com/galaxygeneralrobotics/galbotsdk/blob/main/docs/g1/en/examples_python/index.html

This example demonstrates how to configure and use the SDK's built-in logging system. It shows how to set log file path, name, size limits, console output, and log level. Various log levels (debug, info, warning, error, critical) are then used to write messages.

```python
import galbot_sdk

cfg = {
    # Log storage directory; if empty, defaults to ~/galbot_sdk_log/user_log
    "path": "",
    
    # Log file name; if empty, defaults to <process_name>_<current_time>_<pid>_<thread_id>.log
    "file_name": "",
    
    # log bytes, log size,
    "file_max_size": 10 * 1024 * 1024,  # 10MB
    
    # Maximum rotating log files; oldest file is overwritten when limit exceeded
    "file_max_num": 5,
    
    # Whether to output logs to console; default is False
    "console_output": True,
    
    # Log output level, available values: debug, info, warning, error, critical
    "level": "info",
}
galbot_sdk.init_logger(cfg)

# Write log
galbot_sdk.debug("Debug example")
galbot_sdk.info("Info example")
galbot_sdk.warning("Warning example")
galbot_sdk.error("Error example")
galbot_sdk.critical("Critical example")
```