C++ Examples
This file provides brief examples for the publicly available functions and types in the API, demonstrating how to use these interfaces.
Robot Joint Names (G1 2.2)
Joint Group List
Robot joint group names include: ["head", "left_arm", "right_arm", "leg", "left_gripper", "right_gripper", "left_suction_cup", "right_suction_cup"]
Detailed Information for Each Joint Group
| Joint Group | English Name | Number of Joints | Joint Name List |
|---|---|---|---|
| Head | head | 2 | head_joint1, head_joint2 |
| Legs | leg | 5 | leg_joint1, leg_joint2, leg_joint3, leg_joint4, leg_joint5 |
| Left Arm | 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 | 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 | left_gripper | 1 | left_gripper_joint1 |
| Right Gripper | right_gripper | 1 | right_gripper_joint1 |
| Left Suction Cup | left_suction_cup | 1 | left_suction_cup_joint1 |
| Right Suction Cup | right_suction_cup | 1 | right_suction_cup_joint1 |
Class: GalbotRobot
Tips: If you get data immediately after program startup, the data may not be ready right away. You may sleep for a few seconds as appropriate.
Get Instance and Initialize (get_instance && init)
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::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化单例对象
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 判断是否处于运行状态
while (robot.is_running()) {
// do something
std::cout << "系统正在运行。" << std::endl;
break;
}
// 注册退出回调(可选项，收到退出信号时会自动触发)
robot.register_exit_callback([]() {
std::cout << "系统正在退出..." << std::endl;
});
std::cout << "成功注册系统退出回调" << std::endl;
// 发出退出信号
robot.request_shutdown();
// 等待进入shutdown状态
robot.wait_for_shutdown();
// 释放SDK相关资源
robot.destroy();
std::cout << "程序结束" << std::endl;
return 0;
}
Release SDK Resources and Exit Program
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::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 停止底盘运动
while (true) {
ControlStatus status = robot.stop_base();
if (status == ControlStatus::SUCCESS) {
std::cout << "底盘已成功停止运动！" << std::endl;
break;
} else {
std::cerr << "底盘停止运动失败，正在重试..." << std::endl;
}
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Log interface
examples/cpp/galbot_robot/src/logger_example.cpp
#include <iostream>
#include "galbot_robot.hpp"
#include "galbot_sdk_logger.hpp"
using namespace galbot::sdk::g1;
int main() {
// 获取 GalbotRobot 的单例并初始化
auto& robot = GalbotRobot::get_instance();
robot.init();
// ==============================
// 1. 初始化 SDK 日志
// ==============================
LoggerConfig logger_config;
// 日志生成路径，如不填充，默认生成在~/galbot_sdk_log/user_log路径
logger_config.path = "";
// 日志文件名，如不填充，默认日志名称为<process_name>_<current_time>_<pid>_<thread_id>.log
logger_config.file_name = "";
// 单个日志文件最大字节数
logger_config.file_max_size = 1024 * 1024 * 10; // 10MB
// 循环日志文件最大数量，单个文件写满后新建文件，最多保留 file_max_num 个文件
logger_config.file_max_num = 5;
// 输出日志最低等级
logger_config.level = LogLevel::DEBUG;
// 是否输出到终端，默认为 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. 打印不同级别日志
// ==============================
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. 支持链式 << 风格
// ==============================
int robot_id = 3;
GALBOT_SDK_LOG_INFO << "robot_id = " << robot_id;
GALBOT_SDK_LOG_INFO << "example program finished";
// 退出系统并进行SDK资源、logger资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Set Joint Positions (set_joint_positions)
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::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取指定关节名称，关节组名称包括["leg", "head", "left_arm", "right_arm"]
std::vector<std::string> joint_groups = {"head"};
bool only_active_joint = true; // 获取可活动关节
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;
}
// 传入空数组默认获取所有关节组信息
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 = {"head"};
// 要控制的指定关节，如填充将覆盖joint_groups参数
std::vector<std::string> joint_names = {};
// 关节位置，头部关节组包含两个关节
std::vector<double> joint_pos = {0.2, 0.2};
// 是否阻塞等待关节角度执行到位或超时
bool is_block = true;
// 关节最大运行速度（rad/s）
double speed_rad_s = 0.1;
// 最大等待时间（秒）
double timeout_s = 10.0;
// 设置关节位置
galbot::sdk::g1::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 << "关节命令设置成功！" << std::endl;
} else {
std::cerr << "关节命令设置失败！" << std::endl;
}
// 根据关节组查询关节位置，传入空数组默认填充腿、头、双臂关节组。第二个参数为指定关节名称，如填写将覆盖joint_groups参数。
auto ret_positions = robot.get_joint_positions(joint_groups, {});
for (auto position : ret_positions) {
std::cout << "joint positions is " << position << std::endl;
}
// 使用特定关节名称进行控制，该参数将覆盖joint_groups关节组参数
joint_names = {"head_joint1", "head_joint2"};
joint_pos = {0.0, 0.0};
// 设置关节位置
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 << "关节命令设置成功！" << std::endl;
} else {
std::cerr << "关节命令设置失败！" << std::endl;
}
// 根据关节组查询关节位置，传入空数组默认填充腿、头、双臂关节组。第二个参数为指定关节名称，如填写将覆盖joint_groups参数。
ret_positions = robot.get_joint_positions(joint_groups, {});
for (auto position : ret_positions) {
std::cout << "joint positions is " << position << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Set Gripper Command (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::g1;
void print_gripper_state(
std::shared_ptr<galbot::sdk::g1::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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 夹爪宽度（米）
double width_m = 0.02;
// 夹爪速度（米/秒）
double velocity_mps = 0.05;
// 夹爪力矩（牛米）
double effort = 10;
// 是否阻塞等待执行完成
bool is_blocking = true;
// 设置左夹爪宽度为0.02米，以0.05米速度执行，力矩为10，并阻塞等待执行完成
galbot::sdk::g1::ControlStatus gripper_execution_status =
robot.set_gripper_command(JointGroup::LEFT_GRIPPER, width_m, velocity_mps,
effort, is_blocking);
if (gripper_execution_status == ControlStatus::SUCCESS) {
std::cout << "夹爪命令设置成功！" << std::endl;
} else {
std::cerr << "夹爪命令设置失败！" << std::endl;
}
// 获取夹爪状态
galbot::sdk::g1::JointStateMessage joint_state;
auto gripper_state_ptr = robot.get_gripper_state(JointGroup::LEFT_GRIPPER);
if (gripper_state_ptr == nullptr) {
std::cerr << "get gripper state error" << std::endl;
} else {
print_gripper_state(gripper_state_ptr);
}
// 夹爪宽度（米）
width_m = 0.1;
// 夹爪速度（米/秒）
velocity_mps = 0.05;
// 夹爪力矩（牛米）
effort = 10;
// 是否阻塞等待执行完成
is_blocking = false;
// 设置左夹爪宽度为0.1米，以0.05米速度执行，力矩为10，并阻塞等待执行完成
gripper_execution_status =
robot.set_gripper_command(JointGroup::LEFT_GRIPPER, width_m, velocity_mps,
effort, is_blocking);
if (gripper_execution_status == ControlStatus::SUCCESS) {
std::cout << "夹爪命令设置成功！" << std::endl;
} else {
std::cerr << "夹爪命令设置失败！" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Set Suction Cup Command (set_suction_cup_command)
examples/cpp/galbot_robot/src/set_suction_cup_command_example.cpp
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 激活吸盘
if (robot.set_suction_cup_command(JointGroup::RIGHT_SUCTION_CUP, true) == ControlStatus::SUCCESS) {
std::cout << "吸盘激活指令发送成功" << std::endl;
} else {
std::cerr << "吸盘激活指令发送失败！" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 关闭吸盘
if (robot.set_suction_cup_command(JointGroup::RIGHT_SUCTION_CUP, false) == ControlStatus::SUCCESS) {
std::cout << "吸盘关闭指令发送成功" << std::endl;
} else {
std::cerr << "吸盘关闭指令发送失败" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Stop Trajectory Execution (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::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 发送停止轨迹执行指令
while(true) {
galbot::sdk::g1::ControlStatus joint_execution_status =
robot.stop_trajectory_execution();
// 检查执行结果
if (joint_execution_status == ControlStatus::SUCCESS) {
std::cout << "停止轨迹执行指令发送成功" << std::endl;
break;
} else {
std::cerr << "停止轨迹执行指令发送失败，重试中..." << std::endl;
}
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Set Trajectory (execute_joint_trajectory)
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::g1;
double g_target_time = 10;
double g_start_time = 10;
std::string trajectory_status_to_string(
galbot::sdk::g1::TrajectoryControlStatus status) {
switch (status) {
case galbot::sdk::g1::TrajectoryControlStatus::INVALID_INPUT:
return "INVALID_INPUT";
case galbot::sdk::g1::TrajectoryControlStatus::RUNNING:
return "RUNNING";
case galbot::sdk::g1::TrajectoryControlStatus::COMPLETED:
return "COMPLETED";
case galbot::sdk::g1::TrajectoryControlStatus::STOPPED_UNREACHED:
return "STOPPED_UNREACHED";
case galbot::sdk::g1::TrajectoryControlStatus::ERROR:
return "ERROR";
case galbot::sdk::g1::TrajectoryControlStatus::DATA_FETCH_FAILED:
return "DATA_FETCH_FAILED";
case galbot::sdk::g1::TrajectoryControlStatus::STATUS_NUM:
return "STATUS_NUM";
default:
return "UNKNOWN_STATUS";
}
}
void wait_for_traj_reached(const std::vector<std::string> &joint_groups) {
std::vector<galbot::sdk::g1::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 = galbot::sdk::g1::GalbotRobot::get_instance()
.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] !=
galbot::sdk::g1::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<galbot::sdk::g1::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<galbot::sdk::g1::TrajectoryPoint> trajectory_data_vec;
trajectory_data_vec.resize(step + 1);
// 创建 RobotCommand 轨迹
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);
// 添加关节命令
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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 执行关节轨迹
Trajectory trajectory;
// 填写要控制的关节组名称，关节组名称包括["leg", "head", "left_arm", "right_arm"，"left_gripper", "right_gripper"]
trajectory.joint_groups = {"head"};
// 如需控制指定关节角度，可填写该字段，如填写将覆盖joint_groups字段
trajectory.joint_names = {};
// 生成轨迹
trajectory.points = generate_target_trajectory(2);
// 是否阻塞等待轨迹执行完成，false时可使用
bool is_traj_block = false;
// 等待轨迹执行完成，此函数封装了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（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"
void print_joint_positions(const std::vector<double>& positions) {
std::cout << "当前关节位置：" << std::endl;
for (size_t i = 0; i < positions.size(); ++i) {
std::cout << " 关节 " << i << ": " << positions[i] << " rad" << std::endl;
}
std::cout << std::endl;
}
std::string execution_status_to_string(galbot::sdk::g1::ControlStatus status) {
switch (status) {
case galbot::sdk::g1::ControlStatus::SUCCESS:
return "SUCCESS";
case galbot::sdk::g1::ControlStatus::TIMEOUT:
return "TIMEOUT";
case galbot::sdk::g1::ControlStatus::FAULT:
return "FAULT";
case galbot::sdk::g1::ControlStatus::INVALID_INPUT:
return "INVALID_INPUT";
case galbot::sdk::g1::ControlStatus::INIT_FAILED:
return "INIT_FAILED";
case galbot::sdk::g1::ControlStatus::IN_PROGRESS:
return "IN_PROGRESS";
case galbot::sdk::g1::ControlStatus::STOPPED_UNREACHED:
return "STOPPED_UNREACHED";
case galbot::sdk::g1::ControlStatus::DATA_FETCH_FAILED:
return "DATA_FETCH_FAILED";
case galbot::sdk::g1::ControlStatus::PUBLISH_FAIL:
return "PUBLISH_FAIL";
case galbot::sdk::g1::ControlStatus::COMM_DISCONNECTED:
return "COMM_DISCONNECTED";
default:
return "UNKNOWN_STATUS";
}
}
int main() {
// 获取对象实例
auto& robot = galbot::sdk::g1::GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
std::vector<std::string> joint_groups = {"head"};
std::vector<std::string> joint_names = {};
std::vector<galbot::sdk::g1::JointCommand> joint_commands(2);
joint_commands[0].position = 0.2;
joint_commands[1].position = 0.2;
// 设置头部关节弧度为0.3 0.3
galbot::sdk::g1::ControlStatus execution_status =
robot.set_joint_commands(joint_commands, joint_groups, joint_names);
if (execution_status != galbot::sdk::g1::ControlStatus::SUCCESS) {
std::cout << "关节角指令发送失败" << std::endl;
} else {
std::cout << "关节角指令发送成功" << std::endl;
}
// 稍等一会儿，等待动作执行完成
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
// 查询关节位置
auto ret_positions = robot.get_joint_positions(joint_groups, {});
print_joint_positions(ret_positions);
// 第二步：回到初始位置 —— 将两个头部关节都设为 0.0 rad
joint_commands[0].position = 0.0;
joint_commands[1].position = 0.0;
// 使用关节名称设置关节命令，如设置joint_names字段将会覆盖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 != galbot::sdk::g1::ControlStatus::SUCCESS) {
std::cout << "关节角指令发送失败" << std::endl;
} else {
std::cout << "关节角指令发送成功" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
std::cout << "\n程序已退出" << std::endl;
return 0;
}
Set Joint Commands in Batch Mode (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::g1;
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);
// 创建批量轨迹点
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);
// 添加关节命令
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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 批量设置关节指令
Trajectory trajectory;
// 填写要控制的关节组名称，关节组名称包括["leg", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]
trajectory.joint_groups = {"head"};
// 如需控制指定关节角度，可填写该字段，如填写将覆盖joint_groups字段
trajectory.joint_names = {};
// 生成批量轨迹点（包含多个时间点的关节指令）
trajectory.points = generate_batch_trajectory(2, 0.2, 10.0, 10);
// 批量设置关节指令（非阻塞，立即返回）
ControlStatus status = robot.set_joint_commands_batch(trajectory);
std::cout << "批量设置关节指令状态: " << control_status_to_string(status) << std::endl;
if (status == ControlStatus::SUCCESS) {
std::cout << "批量指令已提交，正在后台执行（非阻塞）" << std::endl;
} else {
std::cerr << "批量指令提交失败！" << std::endl;
}
// 等待一段时间让指令执行
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Stop Base Motion (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::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 停止底盘运动
while (true) {
ControlStatus status = robot.stop_base();
if (status == ControlStatus::SUCCESS) {
std::cout << "底盘已成功停止运动！" << std::endl;
break;
} else {
std::cerr << "底盘停止运动失败，正在重试..." << std::endl;
}
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Set Base Velocity (set_base_velocity)
examples/cpp/galbot_robot/src/set_base_velocity_example.cpp
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 请确认周边环境再进行底盘测试
// 设置底盘速度，linear_velocity前两个字段为底盘x与y方向速度，angular_velocity第三个字段为z方向旋转速度
std::array<double, 3> linear_velocity = {0.05, 0.0, 0.0}; // 前进 0.05 m/s
std::array<double, 3> angular_velocity = {0.0, 0.0, 0.1}; // 旋转 0.1 rad/s
double duration_s = 2.0; // 持续 2 秒后自动停止
if (robot.set_base_velocity(linear_velocity, angular_velocity, duration_s) == ControlStatus::SUCCESS) {
std::cout << "底盘速度设置成功，将在 " << duration_s << " 秒后自动停止。" << std::endl;
} else {
std::cerr << "设置底盘速度失败。" << std::endl;
}
// 等待自动停止完成（预留少量缓冲时间）
std::this_thread::sleep_for(std::chrono::duration<double>(duration_s + 0.5));
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Joint States (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::g1;
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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 使用关节组名称获取关节状态，为空默认返回所有关节
std::vector<std::string> joint_groups = {"left_arm"};
auto ret_states = robot.get_joint_states(joint_groups, {});
print_joint_states(ret_states);
// 获取指定关节状态，如果填充将覆盖关节组输入
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);
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Joint Positions (get_joint_positions)
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::g1;
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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 使用关节组名称获取关节位置，为空默认返回所有关节
std::vector<std::string> joint_groups = {"left_arm"};
auto ret_positions = robot.get_joint_positions(joint_groups, {});
std::cout << "左臂关节位置：" << std::endl;
print_joint_positions(ret_positions);
// 获取指定关节位置，如果填充将覆盖关节组输入
std::vector<std::string> joint_names = {"left_arm_joint1", "left_arm_joint2"};
ret_positions = robot.get_joint_positions({joint_groups}, joint_names);
std::cout << "左臂关节1及关节2位置：" << std::endl;
print_joint_positions(ret_positions);
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Joint Names (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::g1;
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取指定关节名称，关节组名称包括["leg", "head", "left_arm", "right_arm"]
std::vector<std::string> joint_groups = {"head"};
bool only_active_joint = true; // 获取可活动关节
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;
}
// 传入空数组默认获取所有关节组信息
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;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Gripper State (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::g1;
void print_gripper_state(
std::shared_ptr<galbot::sdk::g1::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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取夹爪状态
auto gripper_state_ptr = robot.get_gripper_state(JointGroup::LEFT_GRIPPER);
if (gripper_state_ptr == nullptr) {
std::cerr << "get gripper state error" << std::endl;
} else {
std::cout << "左夹爪状态：" << std::endl;
print_gripper_state(gripper_state_ptr);
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Suction Cup State (get_suction_cup_state)
examples/cpp/galbot_robot/src/get_suction_cup_state_example.cpp
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
void print_suction_cup_state(
std::shared_ptr<galbot::sdk::g1::SuctionCupState> suction_cup_state) {
std::cout << "Timestamp (ns): " << suction_cup_state->timestamp_ns << std::endl;
std::cout << "Activation: " << suction_cup_state->activation << std::endl;
std::cout << "Pressure: " << suction_cup_state->pressure << " Pa" << std::endl;
std::cout << "Action State: " << int(suction_cup_state->action_state) << std::endl;
}
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取吸盘状态
auto suction_cup_state_ptr = robot.get_suction_cup_state(JointGroup::RIGHT_SUCTION_CUP);
if (suction_cup_state_ptr == nullptr) {
std::cerr << "get suction cup state error" << std::endl;
} else {
std::cout << "右吸盘状态：" << std::endl;
print_suction_cup_state(suction_cup_state_ptr);
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Transform (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::g1;
void print_pose_vec(const std::vector<double> &pose_vec) {
// 输出 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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取坐标变换
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);
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get IMU Data (get_imu_data)
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::g1;
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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取imu数据
std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::TORSO_IMU);
if (imu_data) {
std::cout << "IMU数据获取成功！" << std::endl;
print_imu_data(imu_data);
} else {
std::cerr << "IMU数据获取失败！" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Power Information（get_bms_information）
examples/cpp/galbot_robot/src/get_bms_information_example.cpp
#include <iostream>
#include <chrono>
#include <thread>
#include <string>
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
void print_bms_information(const std::shared_ptr<BmsInfo>& bms_info) {
if (!bms_info) {
std::cerr << "BMS信息为空" << std::endl;
return;
}
std::cout << "Voltage (V): " << bms_info->voltage << std::endl;
std::cout << "Current (A): " << bms_info->current << std::endl;
std::cout << "Battery level (%): " << bms_info->battery_level << std::endl;
std::cout << "Temperature (C): " << bms_info->temperature << std::endl;
std::cout << "Charging status: " << std::boolalpha << bms_info->charging_status
<< std::noboolalpha << std::endl;
std::cout << "Health status: " << std::boolalpha << bms_info->health_status
<< std::noboolalpha << std::endl;
std::cout << "Capacity (Ah): " << bms_info->capacity << std::endl;
}
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 获取BMS信息
auto bms_info = robot.get_bms_information();
if (bms_info) {
std::cout << "BMS信息获取成功！" << std::endl;
print_bms_information(bms_info);
} else {
std::cerr << "BMS信息获取失败！" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Device Information (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::g1;
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 << "设备信息：" << std::endl;
std::cout << " 型号: " << (device_info->model.empty() ? "N/A" : device_info->model) << std::endl;
std::cout << " 序列号: " << (device_info->serial_number.empty() ? "N/A" : device_info->serial_number) << std::endl;
std::cout << " 固件版本: " << (device_info->firmware_version.empty() ? "N/A" : device_info->firmware_version)
<< std::endl;
std::cout << " 硬件版本: " << (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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 获取设备信息
std::shared_ptr<DeviceInfo> device_info = robot.get_device_information();
if (device_info) {
std::cout << "设备信息获取成功！" << std::endl;
print_device_info(device_info);
} else {
std::cerr << "设备信息获取失败！" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Camera Image Data (get_rgb_data && get_depth_data) (get_camera_data)
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::g1;
void print_rgb_data(
const std::shared_ptr<galbot::sdk::g1::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 << "图片已保存至 result_image.jpg" << std::endl;
}
void print_depth_data(
const std::shared_ptr<galbot::sdk::g1::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;
// 图片信息归一化
cv::normalize(*img, img_vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
// 伪彩色增强
cv::Mat img_color;
cv::applyColorMap(img_vis, img_color, cv::COLORMAP_JET);
// 保存图片
cv::imwrite("check_raw_data.png", *img);
cv::imwrite("check_visual_view.jpg", img_color);
std::cout << "图片保存完毕：\n"
<< "1. check_raw_data.png -> 包含真实物理深度的全黑图\n"
<< "2. check_visual_view.jpg -> 能看清物体轮廓的彩色图" << std::endl;
}
}
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化传感器，为节省资源，只有初始化中传入的相机与雷达传感器可获取数据
std::unordered_set<SensorType> sensor_types = {
SensorType::HEAD_LEFT_CAMERA, // 头部左相机
SensorType::LEFT_ARM_DEPTH_CAMERA, // 左臂深度相机
};
// 初始化系统
if (robot.init(sensor_types)) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 等待相机数据就绪
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取 RGB 图像数据
std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::HEAD_LEFT_CAMERA);
if (rgb_data) {
std::cout << "RGB 图像数据获取成功！" << std::endl;
print_rgb_data(rgb_data);
} else {
std::cerr << "RGB 图像数据获取失败！" << std::endl;
}
// 获取深度图像数据
std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
if (depth_data) {
std::cout << "深度图像数据获取成功！" << std::endl;
print_depth_data(depth_data);
} else {
std::cerr << "深度图像数据获取失败！" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Sensor Parameters (get_camera_intrinsic && get_sensor_extrinsic)
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::g1;
void print_rgb_data(
const std::shared_ptr<galbot::sdk::g1::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 << "图片已保存至 result_image.jpg" << std::endl;
}
void print_depth_data(
const std::shared_ptr<galbot::sdk::g1::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;
// 图片信息归一化
cv::normalize(*img, img_vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
// 伪彩色增强
cv::Mat img_color;
cv::applyColorMap(img_vis, img_color, cv::COLORMAP_JET);
// 保存图片
cv::imwrite("check_raw_data.png", *img);
cv::imwrite("check_visual_view.jpg", img_color);
std::cout << "图片保存完毕：\n"
<< "1. check_raw_data.png -> 包含真实物理深度的全黑图\n"
<< "2. check_visual_view.jpg -> 能看清物体轮廓的彩色图" << 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() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化传感器，为节省资源，只有初始化中传入的相机与雷达传感器可获取数据
std::unordered_set<SensorType> sensor_types = {
SensorType::HEAD_LEFT_CAMERA, // 头部左相机
SensorType::LEFT_ARM_DEPTH_CAMERA, // 左臂深度相机
};
// 初始化系统
if (robot.init(sensor_types)) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 等待相机数据就绪
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取 RGB 图像数据
std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::HEAD_LEFT_CAMERA);
if (rgb_data) {
std::cout << "RGB 图像数据获取成功！" << std::endl;
print_rgb_data(rgb_data);
} else {
std::cerr << "RGB 图像数据获取失败！" << std::endl;
}
// 获取相机参数
std::shared_ptr<CameraInfo> rgb_camerainfo = robot.get_camera_intrinsic(SensorType::LEFT_ARM_DEPTH_CAMERA);
if (rgb_camerainfo) {
std::cout << "相机参数获取成功！" << std::endl;
print_camera_info(rgb_camerainfo);
} else {
std::cerr << "相机参数获取失败！" << std::endl;
}
// 获取深度图像数据
std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
if (depth_data) {
std::cout << "深度图像数据获取成功！" << std::endl;
print_depth_data(depth_data);
} else {
std::cerr << "深度图像数据获取失败！" << std::endl;
}
// 获取传感器外参
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 << "传感器外参获取成功！" << 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 << "传感器外参获取失败！" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Lidar Data (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::g1;
// 定义点结构体
struct Point3D {
float x, y, z;
};
/**
* 直接通过指针操作从 LidarData 中提取 XYZ 数组
*/
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. 查找 x, y, z 字段的偏移量 (offset)
// Python 版中通过 field name 索引，这里我们预先找到偏移量以提高效率
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 << "错误: 点云数据缺少必要的 xyz 字段" << 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. 直接指针读取（零拷贝核心逻辑）
for (uint32_t i = 0; i < num_points; ++i) {
// 计算当前点的起始指针
const uint8_t* pt_ptr = raw_data + (i * point_step);
// 使用 reinterpret_cast 直接转换指针类型读取内存
// 假设雷达数据是 float32 (F)，这是最常见的格式
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);
// 处理 NaN (对应 Python 的 remove_nan 逻辑)
if (remove_nan) {
if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
continue;
}
}
points.push_back({x, y, z});
}
return points;
}
/**
* 保存到 PCD 文件
*/
void save_xyz_to_pcd(const std::vector<Point3D>& points, const std::string& filename) {
std::ofstream fs(filename);
if (!fs.is_open()) {
std::cerr << "无法打开文件进行写入: " << 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 << "已保存 " << points.size() << " 个点到 " << filename << std::endl;
}
int main() {
// 获取对象实例
auto& robot = GalbotRobot::get_instance();
// 初始化传感器，为节省资源，只有初始化中传入的相机与雷达传感器可获取数据
std::unordered_set<SensorType> sensor_types = {
SensorType::BASE_LIDAR // 底盘激光雷达
};
// 初始化系统
if (robot.init(sensor_types)) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 获取激光雷达数据
std::shared_ptr<LidarData> lidar_data = robot.get_lidar_data(SensorType::BASE_LIDAR);
if (lidar_data) {
std::cout << "激光雷达数据获取成功！" << 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 << "激光雷达数据获取失败！" << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Whole-Body One-Shot Execution Interface – Chassis Velocity Control (whole_body_execution_vel)
examples/cpp/galbot_robot/src/whole_body_execution_vel_example.cpp
#include <array>
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
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();
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
std::vector<double> whole_body_joint_1 = {
0.3, 1.2, 0.85, 0.0, 0.0, // leg
0.5, 0.5, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.0, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.0 // right_arm
};
std::vector<double> whole_body_joint_2 = {
0.3, 1.2, 0.85, 0.0, 0.0, // leg
0.0, 0.0, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.0, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.0 // right_arm
};
// Base velocity command (twist)
std::array<double, 3> linear_velocity_1 = {0.1, 0.0, 0.0};
std::array<double, 3> angular_velocity_1 = {0.0, 0.0, 0.0};
std::array<double, 3> linear_velocity_2 = {-0.1, 0.0, 0.0};
std::array<double, 3> angular_velocity_2 = {0.0, 0.0, 0.0};
std::cout << "=== Whole-body + base velocity ===" << std::endl;
ControlStatus vel_status = robot.execute_whole_body_target(
whole_body_joint_1,
linear_velocity_1,
angular_velocity_1,
/*is_blocking=*/false,
/*speed_rad_s=*/0.1,
/*timeout_s=*/10.0);
std::cout << "execute_whole_body_target (twist) status: "
<< control_status_to_string(vel_status) << std::endl;
// 底盘运动1s后停止
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
robot.stop_base();
// 等待5s关节运动完成
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
vel_status = robot.execute_whole_body_target(
whole_body_joint_2,
linear_velocity_2,
angular_velocity_2,
/*is_blocking=*/false,
/*speed_rad_s=*/0.1,
/*timeout_s=*/10.0);
std::cout << "execute_whole_body_target (twist) status: "
<< control_status_to_string(vel_status) << std::endl;
// 底盘运动1s后停止
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
robot.stop_base();
// 等待5s关节运动完成
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Whole-Body One-Shot Execution Interface – Chassis Position Control (Odometry Frame) (whole_body_execution_odom)
examples/cpp/galbot_robot/src/whole_body_execution_odom_example.cpp
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
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();
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
std::vector<double> whole_body_joint_1 = {
0.3, 1.2, 0.85, 0.0, 0.0, // leg
0.5, 0.5, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.0, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.0 // right_arm
};
std::vector<double> whole_body_joint_2 = {
0.3, 1.2, 0.85, 0.0, 0.0, // leg
0.0, 0.0, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.0, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.0 // right_arm
};
// Base pose command odom(x, y, yaw)
double base_x_1 = 1.0;
double base_y_1 = 0.0;
double base_yaw_1 = 0.0;
double base_x_2 = 0.0;
double base_y_2 = 0.0;
double base_yaw_2 = 0.0;
// 可选坐标系（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";
// 底盘位姿插值时间（秒），用于生成平滑底盘轨迹
const double base_time_s = 10.0;
// 阻塞等待超时时间（秒），建议设置为大于底盘位姿插值时间
const double timeout_s = 15.0;
std::cout << "=== Whole-body + base pose (odom) ===" << std::endl;
ControlStatus pose_status = robot.execute_whole_body_target(
whole_body_joint_1,
base_x_1,
base_y_1,
base_yaw_1,
frame_id,
reference_frame_id,
/*is_blocking=*/true,
/*speed_rad_s=*/0.1,
/*time_from_start_s=*/base_time_s,
/*timeout_s=*/timeout_s);
std::cout << "execute_whole_body_target (pose) status: "
<< control_status_to_string(pose_status) << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
pose_status = robot.execute_whole_body_target(
whole_body_joint_2,
base_x_2,
base_y_2,
base_yaw_2,
frame_id,
reference_frame_id,
/*is_blocking=*/true,
/*speed_rad_s=*/0.1,
/*time_from_start_s=*/base_time_s,
/*timeout_s=*/timeout_s);
std::cout << "execute_whole_body_target (pose) status: "
<< control_status_to_string(pose_status) << std::endl;
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Whole-Body One-Shot Execution Interface – Chassis Position Control (Map Frame) (whole_body_execution_map)
examples/cpp/galbot_robot/src/whole_body_execution_map_example.cpp
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
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();
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
std::vector<double> whole_body_joint_1 = {
0.3, 1.2, 0.85, 0.0, 0.0, // leg
0.5, 0.5, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.0, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.0 // right_arm
};
std::vector<double> whole_body_joint_2 = {
0.3, 1.2, 0.85, 0.0, 0.0, // leg
0.0, 0.0, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.0, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.0 // right_arm
};
// Base pose command map(x, y, yaw) 注意：需要根据实际环境机器人在map坐标系下的定位进行调整
double base_x_1 = -0.4;
double base_y_1 = 0.226593;
double base_yaw_1 = 0.0;
double base_x_2 = -0.60583;
double base_y_2 = 0.226593;
double base_yaw_2 = 0.0;
// 可选坐标系（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";
// 底盘位姿插值时间（秒），用于生成平滑底盘轨迹
const double base_time_s = 10.0;
// 阻塞等待超时时间（秒），建议设置为大于底盘位姿插值时间
const double timeout_s = 15.0;
std::cout << "=== Whole-body + base pose (map) ===" << std::endl;
ControlStatus pose_status = robot.execute_whole_body_target(
whole_body_joint_1,
base_x_1,
base_y_1,
base_yaw_1,
frame_id,
reference_frame_id,
/*is_blocking=*/true,
/*speed_rad_s=*/0.1,
/*time_from_start_s=*/base_time_s,
/*timeout_s=*/timeout_s);
std::cout << "execute_whole_body_target (pose) status: "
<< control_status_to_string(pose_status) << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
pose_status = robot.execute_whole_body_target(
whole_body_joint_2,
base_x_2,
base_y_2,
base_yaw_2,
frame_id,
reference_frame_id,
/*is_blocking=*/true,
/*speed_rad_s=*/0.1,
/*time_from_start_s=*/base_time_s,
/*timeout_s=*/timeout_s);
std::cout << "execute_whole_body_target (pose) status: "
<< control_status_to_string(pose_status) << std::endl;
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
One-Click Reset to Zero – Odometry Frame (whole_body_reset_zero_odom)
examples/cpp/galbot_robot/src/whole_body_reset_zero_odom_example.cpp
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
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();
auto& motion = GalbotMotion::get_instance();
if (!robot.init()) {
std::cerr << "GalbotRobot init failed." << std::endl;
return -1;
}
if (!motion.init()) {
std::cerr << "GalbotMotion init failed." << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
std::vector<double> whole_body_joint_1 = {
0.25, 1.1, 0.85, 0.0, 0.0, // leg
0.5, 0.5, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.2, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.2 // right_arm
};
// Base pose command odom(x, y, yaw)
double base_x_1 = 0.2;
double base_y_1 = 0.0;
double base_yaw_1 = 0.0;
// 可选坐标系（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";
// 底盘位姿插值时间（秒），用于生成平滑底盘轨迹
const double base_time_s = 10.0;
// 阻塞等待超时时间（秒），建议设置为大于底盘位姿插值时间
const double timeout_s = 15.0;
std::cout << "=== Whole-body + base pose (odom) ===" << std::endl;
ControlStatus pose_status = robot.execute_whole_body_target(
whole_body_joint_1,
base_x_1,
base_y_1,
base_yaw_1,
frame_id,
reference_frame_id,
/*is_blocking=*/true,
/*speed_rad_s=*/0.1,
/*time_from_start_s=*/base_time_s,
/*timeout_s=*/timeout_s);
std::cout << "execute_whole_body_target (pose) status: "
<< control_status_to_string(pose_status) << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 一键回零
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;
}
One-Click Reset to Zero – Map Frame (whole_body_reset_zero_map)
examples/cpp/galbot_robot/src/whole_body_reset_zero_map_example.cpp
#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
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();
auto& motion = GalbotMotion::get_instance();
if (!robot.init()) {
std::cerr << "GalbotRobot init failed." << std::endl;
return -1;
}
if (!motion.init()) {
std::cerr << "GalbotMotion init failed." << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
// std::vector<double> whole_body_joint_1(21, 0.0);
std::vector<double> whole_body_joint_1 = {
0.25, 1.1, 0.85, 0.0, 0.0, // leg
0.5, 0.5, // head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.2, // left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.2 // right_arm
};
// Base pose command map(x, y, yaw) 注意：需要根据实际环境机器人在map坐标系下的定位进行调整
double base_x_1 = -0.4;
double base_y_1 = 0.226593;
double base_yaw_1 = 0.0;
// Base zero pose: (x, y, yaw) = (0, 0, 0)
// 可选坐标系（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";
// 底盘位姿插值时间（秒），用于生成平滑底盘轨迹
const double base_time_s = 10.0;
// 阻塞等待超时时间（秒），建议设置为大于底盘位姿插值时间
const double timeout_s = 15.0;
std::cout << "=== Whole-body + base pose (map) ===" << std::endl;
ControlStatus pose_status = robot.execute_whole_body_target(
whole_body_joint_1,
base_x_1,
base_y_1,
base_yaw_1,
frame_id,
reference_frame_id,
/*is_blocking=*/true,
/*speed_rad_s=*/0.1,
/*time_from_start_s=*/base_time_s,
/*timeout_s=*/timeout_s);
std::cout << "execute_whole_body_target (pose) status: "
<< control_status_to_string(pose_status) << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 一键回零
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;
}
Class: GalbotMotion
Get Instance and Initialize (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::g1;
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
if (planner.init()) {
std::cout << "规划器初始化成功！" << std::endl;
} else {
std::cerr << "规划器初始化失败！" << std::endl;
return -1;
}
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 仍然可以通过 GalbotRobot 管理机器人生命周期
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Forward Kinematics (Using Current State or Specified 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::g1;
// 辅助打印函数
void print_pose(const std::string& label, const std::tuple<MotionStatus, std::vector<double>>& res,
GalbotMotion& planner) {
std::cout << "[" << label << "] 状态: " << planner.status_to_string(std::get<0>(res)) << std::endl;
if (std::get<0>(res) == MotionStatus::SUCCESS) {
std::cout << "末端位姿: ";
for (double v : std::get<1>(res)) {
std::cout << v << " ";
}
std::cout << "\n" << std::endl;
} else {
std::cout << "计算失败！" << std::endl;
}
}
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
if (planner.init()) {
std::cout << "规划器初始化成功！" << std::endl;
} else {
std::cerr << "规划器初始化失败！" << std::endl;
return -1;
}
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
std::map<std::string, std::vector<double>> chain_joints = {
{"leg", {0.4992, 1.4991, 1.0005, 0.0000}},
{"head", {0.0000, 0.0}},
{"left_arm", {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
{"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}};
std::vector<double> whole_body_joint;
std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
for (const auto& key : keys) {
whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
}
std::vector<double> base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
std::string end_link = "left_arm_end_effector_mount_link";
std::string reference_frame = "base_link";
// --- 测试用例 1: 默认参数 (使用当前机器人状态) ---
try {
std::cout << ">> 正在执行: 基础版正运动学..." << std::endl;
auto res1 = planner.forward_kinematics(end_link, reference_frame);
print_pose("基础版", res1, planner);
} catch (const std::exception& e) {
std::cerr << "❌ 基础版异常: " << e.what() << std::endl;
}
// --- 测试用例 2: 自定义关节状态 + 参数 ---
try {
std::cout << ">> 正在执行: 自定义关节正运动学..." << 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("自定义参数", res2, planner);
} catch (const std::exception& e) {
std::cerr << "❌ 自定义参数异常: " << e.what() << std::endl;
}
// --- 测试用例 3: 基于 RobotStates 的正运动学（使用当前机体状态，保证维数与顺序与规划器一致）---
try {
std::cout << ">> 正在执行: 基于 RobotStates 正运动学..." << std::endl;
RobotStates current_state = planner.getRobotStates();
if (current_state.whole_body_joint.empty()) {
std::cerr << "❌ 基于RobotStates: 无法获取当前机体关节状态，请确认 WBC/传感器已就绪。" << 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("基于RobotStates", res3, planner);
}
} catch (const std::exception& e) {
std::cerr << "❌ 基于RobotStates异常: " << e.what() << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Link Names (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::g1;
void print_link_names(const std::vector<std::string>& link_names, const std::string& title) {
std::cout << title << " (共 " << link_names.size() << " 个):" << std::endl;
for (size_t i = 0; i < link_names.size(); ++i) {
std::cout << " " << (i + 1) << ". " << link_names[i] << std::endl;
}
}
int main() {
// 获取对象实例
auto& motion = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (!motion.init()) {
std::cerr << "GalbotMotion 初始化失败！" << std::endl;
return -1;
}
if (!robot.init()) {
std::cerr << "GalbotRobot 初始化失败！" << std::endl;
return -1;
}
std::cout << "系统初始化成功！" << std::endl;
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
try {
// 获取所有连杆名称
std::vector<std::string> all_link_names = motion.get_link_names(false);
print_link_names(all_link_names, "\n所有连杆名称");
// 只获取末端执行器连杆名称
std::vector<std::string> ee_link_names = motion.get_link_names(true);
print_link_names(ee_link_names, "\n末端执行器连杆名称");
} catch (const std::exception& e) {
std::cerr << "获取连杆名称异常: " << e.what() << std::endl;
}
// 退出系统并进行SDK资源释放
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Inverse Kinematics (Basic and RobotStates-based) (inverse_kinematics)
examples/cpp/galbot_motion/src/inverse_kinematics_example.cpp
#include <algorithm>
#include <chrono>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>
#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
// 辅助函数：打印逆运动学求解结果
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 << "] 状态反馈: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
std::cout << "✅ 逆解计算成功！得到的关节角度如下:" << std::endl;
for (const auto& [name, joints] : joint_map) {
std::cout << " - 链条 [" << name << "]: ";
for (double v : joints)
std::cout << v << " ";
std::cout << std::endl;
}
} else {
std::cout << "❌ 逆解计算失败，请检查输入或目标位姿是否可达。" << std::endl;
}
std::cout << "---------------------------------------------------" << std::endl;
}
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
if (planner.init()) {
std::cout << "规划器初始化成功！" << std::endl;
} else {
std::cerr << "规划器初始化失败！" << std::endl;
return -1;
}
if (robot.init()) {
std::cout << "系统初始化成功！" << std::endl;
} else {
std::cerr << "系统初始化失败！" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 关节状态定义
std::unordered_map<std::string, std::vector<double>> chain_joints = {
{"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
{"head", {0.0000, 0.0}},
{"left_arm", {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
{"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}};
// 目标位姿定义 (x, y, z, qx, qy, qz, qw)
std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
{"left_arm", {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}},
{"right_arm", {0.1267, -0.2345, 0.7358, -0.0225, 0.0126, -0.0343, 0.9991}}};
// 全身体关节向量拼接 (Leg -> Head -> Left Arm -> Right Arm)
std::vector<double> whole_body_joint;
std::vector<std::string> key_order = {"leg", "head", "left_arm", "right_arm"};
for (const auto& key : key_order) {
whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
}
// 通用配置
std::string reference_frame = "base_link";
std::string target_frame = "EndEffector";
std::string target_chain = "left_arm";
auto params = std::make_shared<Parameter>();
// 场景 1：单链逆解
try {
std::cout << ">> 执行场景 1：单链逆解测试..." << std::endl;
std::vector<std::string> one_chain = {target_chain};
auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], // 1. target_pose
one_chain // 2. chain_names
// target_frame, // 3. target_frame
// reference_frame, // 4. reference_frame
// {}, // 5. initial_joint_positions (空)
// false, // 6. enable_collision_check (bool)
// params // 7. params (shared_ptr)
);
print_ik_result("单链逆解", res, planner);
std::this_thread::sleep_for(std::chrono::milliseconds(800));
} catch (const std::exception& e) {
std::cerr << "场景 1 异常: " << e.what() << std::endl;
}
// 场景 2：手臂链 + 腰部逆解
try {
std::cout << ">> 执行场景 2：手臂链 + 腰部逆解测试..." << std::endl;
std::vector<std::string> chain_with_torso = {target_chain, "torso"};
auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], chain_with_torso, target_frame,
reference_frame, {}, false, params);
print_ik_result("手臂+腰部逆解", res, planner);
std::this_thread::sleep_for(std::chrono::milliseconds(800));
} catch (const std::exception& e) {
std::cerr << "场景 2 异常: " << e.what() << std::endl;
}
// 场景 3：非法链组合
try {
std::cout << ">> 执行场景 3：非法链组合测试..." << std::endl;
std::vector<std::string> error_chains = {target_chain, "torso", "head"};
auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], error_chains, target_frame,
reference_frame, {}, false, params);
print_ik_result("非法链组合检测", res, planner);
} catch (const std::exception& e) {
std::cerr << "场景 3 异常: " << e.what() << std::endl;
}
// 场景 4：使用参考关节 (initial_joint_positions可指定链关节作为逆解参考值，未指定链关节使用全身关节补全)
try {
std::cout << ">> 执行场景 4：带初始参考值的逆解测试..." << std::endl;
std::vector<std::string> one_chain = {target_chain};
auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], one_chain, target_frame, reference_frame,
chain_joints, false, params);
print_ik_result("带参考值逆解", res, planner);
std::this_thread::sleep_for(std::chrono::milliseconds(800));
} catch (const std::exception& e) {
std::cerr << "场景 4 异常: " << e.what() << std::endl;
}
// 场景 5：基于 RobotStates 的逆解
try {
std::cout << ">> 执行场景 5：基于 RobotStates 的逆解测试..." << std::endl;
// 构造 RobotStates 智能指针
auto ref_state = std::make_shared<RobotStates>();
ref_state->chain_name = target_chain;
ref_state->whole_body_joint = whole_body_joint;
ref_state->base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
std::vector<std::string> one_chain = {target_chain};
auto res = planner.inverse_kinematics_by_state(chain_pose_baselink[target_chain], // 1. target_pose
one_chain, // 2. chain_names
target_frame, // 3. target_frame
reference_frame, // 4. reference_frame
ref_state, // 5. reference_robot_states (shared_ptr)
false, // 6. enable_collision_check (bool)
params // 7. params (shared_ptr)
);
print_ik_result("RobotStates逆解", res, planner);
} catch (const std::exception& e) {
std::cerr << "场景 5 异常: " << e.what() << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
std::cout << "资源已释放。" << std::endl;
return 0;
}
Get and Set End Effector Pose (get_set_end_effort_pos)
examples/cpp/galbot_motion/src/get_set_end_effort_pos_example.cpp
#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>
#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
using namespace galbot::sdk::g1;
// 辅助函数：打印位姿信息
void print_pose_info(const std::string& label, const std::vector<double>& pose) {
if (pose.size() == 7) {
std::cout << "[" << label << "] 位姿: "
<< "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();
auto& robot = GalbotRobot::get_instance();
if (!planner.init()) {
std::cerr << "GalbotMotion 初始化失败" << std::endl;
return -1;
}
if (!robot.init()) {
std::cerr << "GalbotRobot 初始化失败" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
{"leg", {0.0596, -0.0000, 1.0327, 0.5000, 0.5003, 0.4997, 0.5000}},
{"head", {0.0599, 0.0002, 1.4098, -0.7072, 0.0037, 0.0037, 0.7069}},
{"left_arm", {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}},
{"right_arm", {0.097768, -0.226021, 0.8, -0.0117403, -0.0098713, 0.0157502, 0.999758}}
};
std::string reference_frame = "base_link";
std::string target_frame = "EndEffector";
std::string target_chain = "right_arm";
auto custom_param = std::make_shared<Parameter>();
// --- 场景 1：基础版获取末端位姿 ---
try {
std::cout << ">> 场景 1：正在获取基础末端位姿..." << std::endl;
std::string end_ee_link = "right_arm_end_effector_mount_link";
auto res = planner.get_end_effector_pose(end_ee_link, reference_frame);
MotionStatus status = std::get<0>(res);
std::vector<double> pose = std::get<1>(res);
std::cout << "执行状态: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
print_pose_info("基础版", pose);
}
std::this_thread::sleep_for(std::chrono::milliseconds(800));
} catch (const std::exception& e) {
std::cerr << "❌ 场景 1 异常: " << e.what() << std::endl;
}
// --- 场景 2：指定链名 + 自定义frame获取末端位姿 ---
try {
std::cout << ">> 场景 2：正在指定链名获取位姿..." << std::endl;
auto res = planner.get_end_effector_pose_on_chain(target_chain, target_frame, reference_frame);
MotionStatus status = std::get<0>(res);
std::vector<double> pose = std::get<1>(res);
std::cout << "执行状态: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
print_pose_info("指定链名版", pose);
}
std::this_thread::sleep_for(std::chrono::milliseconds(800));
} catch (const std::exception& e) {
std::cerr << "❌ 场景 2 异常: " << e.what() << std::endl;
}
// --- 场景 3：设置末端位姿 ---
try {
std::cout << ">> 场景 3：正在设置末端位姿..." << std::endl;
std::string ee_frame = "right_arm";
std::vector<double> target_pose = chain_pose_baselink[ee_frame];
MotionStatus status = planner.set_end_effector_pose(
target_pose, // 1
ee_frame, // 2
reference_frame, // 3
nullptr, // 4. 重要：如果不参考特定状态，传入 nullptr
false, // 5. enable_collision_check
true, // 6. is_blocking
5.0, // 7. timeout
custom_param // 8. params
);
std::cout << "设置状态: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
std::cout << "✅ 指令发送成功 (阻塞等待模式)" << std::endl;
}
} catch (const std::exception& e) {
std::cerr << "❌ 设置位姿异常: " << e.what() << std::endl;
}
// --- 场景 4：执行结束再次获取末端位姿 ---
try {
std::cout << ">> 场景 4：正在获取基础末端位姿..." << std::endl;
std::string end_ee_link = "right_arm_end_effector_mount_link";
auto res = planner.get_end_effector_pose(end_ee_link, reference_frame);
MotionStatus status = std::get<0>(res);
std::vector<double> pose = std::get<1>(res);
std::cout << "执行状态: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
print_pose_info("基础版", pose);
}
std::this_thread::sleep_for(std::chrono::milliseconds(800));
} catch (const std::exception& e) {
std::cerr << "❌ 场景 4 异常: " << e.what() << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Single Point Motion Planning (Joint Space and Cartesian Space) (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::g1;
// 定义轨迹返回类型以简化代码
using TrajResult = std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<std::vector<double>>>>;
/**
* 辅助函数：打印规划结果
*/
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 << "] 状态: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
if (traj_map.count(chain_name) && !traj_map[chain_name].empty()) {
std::cout << "✅ 规划成功: 轨迹点数 = " << traj_map[chain_name].size() << std::endl;
} else {
std::cout << "⚠️ 状态为 SUCCESS 但轨迹为空（可能已在目标点）" << std::endl;
}
} else {
std::cout << "❌ 规划失败" << std::endl;
}
std::cout << "---------------------------------------" << std::endl;
}
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
if (!planner.init()) {
std::cerr << "GalbotMotion init FAILED" << std::endl;
return -1;
}
if (!robot.init()) {
std::cerr << "GalbotRobot init FAILED" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::unordered_map<std::string, std::vector<double>> chain_joints = {
{"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
{"head", {0.0000, 0.0}},
{"left_arm", {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
{"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}
};
std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
{"leg", {0.0596, -0.0000, 1.0327, 0.5000, 0.5003, 0.4997, 0.5000}},
{"head", {0.0599, 0.0002, 1.4098, -0.7072, 0.0037, 0.0037, 0.7069}},
{"left_arm", {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}},
{"right_arm", {0.1267, -0.2345, 0.7358, -0.0225, 0.0126, -0.0343, 0.9991}}
};
auto params = std::make_shared<Parameter>();
std::string target_chain = "left_arm";
// NOTE:
// - GalbotMotion does NOT provide real-time obstacle perception / automatic environment updates today.
// - When enable_collision_check=true, collision checking uses self-collision + objects you load manually via
// add_obstacle/attach_target_object (including point clouds if you load them explicitly).
// Scenario 1: joint-space planning, target type = joint state
try {
std::cout << ">> Scenario 1: joint-space planning (joint target)..." << std::endl;
// 构造目标关节状态
auto target_joint = std::make_shared<JointStates>();
target_joint->chain_name = target_chain;
target_joint->joint_positions = chain_joints[target_chain];
auto res = planner.motion_plan(
target_joint, // 1. target
nullptr, // 2. start (nullptr表示从当前开始)
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;
// 构造目标位姿状态
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); // 7个0作为起点
auto res = planner.motion_plan(
target_joint,
start_joint, // 2. 指定起始点
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-Waypoint Trajectory Planning (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::g1;
// 轨迹返回类型定义
using TrajResult = std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<std::vector<double>>>>;
/**
* 辅助函数：打印规划结果
*/
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 << "] 状态反馈: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
if (traj_map.count(chain_name) && !traj_map[chain_name].empty()) {
std::cout << "✅ 多点规划成功: 轨迹总点数 = " << traj_map[chain_name].size() << std::endl;
} else {
std::cout << "⚠️ 状态 SUCCESS 但轨迹为空，可能由于目标与当前位姿重合。" << std::endl;
}
} else {
std::cout << "❌ 多点规划失败。" << std::endl;
}
std::cout << "---------------------------------------------------" << std::endl;
}
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
if (!planner.init()) {
std::cerr << "GalbotMotion 初始化失败" << std::endl;
return -1;
}
if (!robot.init()) {
std::cerr << "GalbotRobot 初始化失败" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::seconds(2));
std::unordered_map<std::string, std::vector<double>> chain_joints = {
{"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
{"head", {0.0000, 0.0}},
{"left_arm", {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
{"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}
};
std::vector<double> whole_body_joint;
std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
for (const auto& key : keys) {
whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
}
auto params = std::make_shared<Parameter>();
std::string target_chain = "left_arm";
// --- 场景 1：笛卡尔空间多路点规划 (PoseState 目标) ---
try {
std::cout << ">> 执行场景 1：笛卡尔空间多路点规划..." << std::endl;
auto target_pose_state = std::make_shared<PoseState>();
target_pose_state->chain_name = target_chain;
// 构造路点（3个中间位姿）
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("笛卡尔多点单链规划", res, target_chain, planner);
std::this_thread::sleep_for(std::chrono::milliseconds(800));
} catch (const std::exception& e) { std::cerr << "场景 1 异常: " << e.what() << std::endl; }
// --- 场景 2：关节空间多路点规划 (JointStates 目标) ---
try {
std::cout << ">> 执行场景 2：关节空间多路点规划..." << std::endl;
auto target_joint = std::make_shared<JointStates>();
target_joint->chain_name = target_chain;
// 构造路点（3个中间位姿）
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("关节空间多点", res, target_chain, planner);
} catch (const std::exception& e) { std::cerr << "场景 2 异常: " << e.what() << std::endl; }
// 4. 清理资源
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Collision Detection (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::g1;
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
auto& navigation = GalbotNavigation::get_instance();
// NOTE:
// - GalbotNavigation (galbotNav) may use real-time obstacle perception/avoidance during navigation (deployment dependent).
// - GalbotMotion does NOT provide real-time obstacle perception today; Motion collision uses self-collision +
// manually loaded obstacles (add_obstacle/attach_target_object).
if (!planner.init()) {
std::cerr << "GalbotMotion init FAILED" << std::endl;
return -1;
}
if (!robot.init()) {
std::cerr << "GalbotRobot init FAILED" << std::endl;
return -1;
}
if (!navigation.init()) {
std::cerr << "GalbotNavigation init FAILED" << std::endl;
return -1;
}
std::unordered_map<std::string, std::vector<double>> chain_joints = {
{"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
{"head", {0.0000, 0.0}},
{"left_arm", {1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000}},
{"right_arm", {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000}}
};
std::vector<double> whole_body_joint;
std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
for (const auto& key : keys) {
whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
}
std::vector<double> base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
std::vector<double> bad_left_arm_joint = {1.99995, -1.60004, 0.599905, -1.69994, 0, -0.799924, 0};
auto custom_param = std::make_shared<Parameter>();
try {
std::cout << ">> Running collision check..." << std::endl;
// 构造待检测的RobotStates状态列表
std::vector<std::shared_ptr<RobotStates>> check_states;
// 状态 0: 全身体状态 (RobotStates)
auto state0 = std::make_shared<RobotStates>();
state0->whole_body_joint = whole_body_joint;
state0->base_state = base_state;
check_states.push_back(state0);
// 状态 1: 特定链状态 (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);
// 调用碰撞检测接口
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 << " - 状态 [" << 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 (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::g1;
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
if (!planner.init()) {
std::cerr << "GalbotMotion 初始化失败" << std::endl;
return -1;
}
if (!robot.init()) {
std::cerr << "GalbotRobot 初始化失败" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::seconds(2));
// --- 执行附加工具操作 ---
try {
std::string chain_name = "left_arm";
std::string tool_name = "suction_cup";
MotionStatus status = planner.attach_tool(chain_name, tool_name);
std::cout << "执行状态反馈: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
std::cout << "✅ 成功加载工具: " << tool_name << std::endl;
} else {
std::cerr << "❌ 加载工具失败，请检查工具名称是否在配置文件中定义。" << std::endl;
}
} catch (const std::exception& e) {
std::cerr << "❌ 运行过程中发生异常: " << e.what() << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Detach Tool (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::g1;
int main() {
auto& planner = GalbotMotion::get_instance();
auto& robot = GalbotRobot::get_instance();
if (!planner.init()) {
std::cerr << "GalbotMotion 初始化失败" << std::endl;
return -1;
}
if (!robot.init()) {
std::cerr << "GalbotRobot 初始化失败" << std::endl;
return -1;
}
// 程序立即启动，稍等数据就绪时间
std::this_thread::sleep_for(std::chrono::seconds(2));
// --- 执行卸载工具操作 ---
try {
std::string chain_name = "left_arm";
MotionStatus status = planner.detach_tool(chain_name);
std::cout << "执行状态反馈: " << planner.status_to_string(status) << std::endl;
if (status == MotionStatus::SUCCESS) {
std::cout << "✅ 成功卸载工具。" << std::endl;
} else {
std::cerr << "❌ 卸载工具失败。" << std::endl;
}
} catch (const std::exception& e) {
std::cerr << "❌ 运行过程中发生异常: " << e.what() << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Add/Remove Environment Collision Objects (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::g1;
/*
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();
auto& robot = GalbotRobot::get_instance();
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. 类型
obj_pose, // 3. 位姿
obj_scale, // 4. 缩放 (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";
// 第一次添加
planner.add_obstacle(obstacle_id, obj_type, obj_pose, obj_scale, "", target_frame, "", {}, {}, {}, 0.0, 0.0);
// 第二次添加相同 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;
}
Class: GalbotNavigation
Get Instance / Initialize (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::g1;
int main() {
auto& navigation = GalbotNavigation::get_instance();
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (!robot.init()) {
std::cerr << "Base instance 初始化失败！" << std::endl;
return -1;
}
if (!navigation.init()) {
std::cerr << "Navigation instance 初始化失败！" << std::endl;
return -1;
}
std::cout << "初始化成功！" << std::endl;
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Relocalize / Is Localized / Get Current Pose (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::g1;
int main() {
auto& navigation = GalbotNavigation::get_instance();
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "Base instance 初始化成功！" << std::endl;
} else {
std::cerr << "Base instance 初始化失败！" << std::endl;
return -1;
}
if (navigation.init()) {
std::cout << "Navigation instance 初始化成功！" << std::endl;
} else {
std::cerr << "Navigation instance 初始化失败！" << std::endl;
return -1;
}
Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
// 检查重定位是否成功
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;
// 获取当前位姿
Pose current_pose = navigation.get_current_pose();
std::cout << "当前位姿: 位置(" << 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;
robot.request_shutdown();
robot.wait_for_shutdown();
} else {
std::cout << "relocalization failed, cannot proceed with navigation." << std::endl;
}
robot.destroy();
return 0;
}
Check Path Reachability and Blocking Navigation to Goal (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::g1;
int main() {
auto& navigation = GalbotNavigation::get_instance();
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "Base instance 初始化成功！" << std::endl;
} else {
std::cerr << "Base instance 初始化失败！" << std::endl;
return -1;
}
if (navigation.init()) {
std::cout << "Navigation instance 初始化成功！" << std::endl;
} else {
std::cerr << "Navigation instance 初始化失败！" << std::endl;
return -1;
}
auto res = robot.switch_controller(ControllerName::CHASSIS_POSE_CTRL);
if (res != ControlStatus::SUCCESS) {
std::cerr << "切换控制器失败！" << 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});
// 检查重定位是否成功
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;
// 获取当前位姿
Pose current_pose = navigation.get_current_pose();
std::cout << "当前位姿: 位置(" << 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;
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 是否开启障碍物检查（环境空旷可设置为true）
bool enable_collision_check = false;
// 是否阻塞等待到达
bool is_blocking = true;
// 最大等待到位时间
float timeout_s = 20;
// 循环反复3次导航
int count = 0;
while (count++ < 3) {
std::cout << "第 " << count << " 次导航" << std::endl;
// 检查路径是否可达并导航到目标
if (navigation.check_path_reachability(goal_pose, init_pose)) {
std::cout << "路径可达，导航至目标点位" << std::endl;
NavigationStatus status = navigation.navigate_to_goal(
goal_pose, enable_collision_check, is_blocking, timeout_s);
if (status == NavigationStatus::SUCCESS) {
std::cout << "已到达目标点位" << std::endl;
} else {
std::cout << "导航失败，状态码: " << static_cast<int>(status) << std::endl;
}
} else {
std::cout << "路径不可达，无法导航至目标点位" << std::endl;
}
// 检查路径是否可达并回到起点
if (navigation.check_path_reachability(init_pose, goal_pose)) {
std::cout << "路径可达，导航至起点" << std::endl;
NavigationStatus status = navigation.navigate_to_goal(
init_pose, enable_collision_check, is_blocking, timeout_s);
if (status == NavigationStatus::SUCCESS) {
std::cout << "已到达起点" << std::endl;
} else {
std::cout << "导航失败，状态码: " << static_cast<int>(status) << std::endl;
}
} else {
std::cout << "路径不可达，无法导航至起点" << std::endl;
}
}
// 停止导航
navigation.stop_navigation();
// 再次获取当前位姿
current_pose = navigation.get_current_pose();
std::cout << "当前位姿: 位置(" << 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;
} else {
std::cout << "relocalization failed." << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Non-blocking Navigation + Polling for Arrival (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::g1;
int main() {
auto& navigation = GalbotNavigation::get_instance();
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (robot.init()) {
std::cout << "Base instance 初始化成功！" << std::endl;
} else {
std::cerr << "Base instance 初始化失败！" << std::endl;
return -1;
}
if (navigation.init()) {
std::cout << "Navigation instance 初始化成功！" << std::endl;
} else {
std::cerr << "Navigation instance 初始化失败！" << std::endl;
return -1;
}
auto res = robot.switch_controller(ControllerName::CHASSIS_POSE_CTRL);
if (res != ControlStatus::SUCCESS) {
std::cerr << "切换控制器失败！" << 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});
// 检查重定位是否成功
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;
// 获取当前位姿
Pose current_pose = navigation.get_current_pose();
std::cout << "当前位姿: 位置(" << 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;
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 是否开启障碍物检查（环境空旷可设置为true）
bool enable_collision_check = false;
// 是否阻塞等待到达
bool is_blocking = false;
// 循环反复2次导航,非阻塞等待到达
int count = 0;
while (count++ < 2) {
std::cout << "第 " << count << " 次导航" << std::endl;
// 检查路径是否可达并导航到目标
if (navigation.check_path_reachability(goal_pose, init_pose)) {
std::cout << "路径可达，导航至目标点位" << std::endl;
NavigationStatus status = navigation.navigate_to_goal(
goal_pose, enable_collision_check, is_blocking);
// 等待到达
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 << "已到达目标点位" << std::endl;
} else {
std::cout << "导航失败，未到达目标点位" << std::endl;
}
} else {
std::cout << "路径不可达，无法导航至目标点位" << std::endl;
}
// 检查路径是否可达并回到起点
if (navigation.check_path_reachability(init_pose, goal_pose)) {
std::cout << "路径可达，导航至起点" << std::endl;
NavigationStatus status = navigation.navigate_to_goal(
init_pose, enable_collision_check, is_blocking);
// 等待到达
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 << "已到达目标点位" << std::endl;
} else {
std::cout << "导航失败，未到达目标点位" << std::endl;
}
} else {
std::cout << "路径不可达，无法导航至起点" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
// 停止导航
navigation.stop_navigation();
// 获取当前位姿
current_pose = navigation.get_current_pose();
std::cout << "当前位姿: 位置(" << 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;
} else {
std::cout << "relocalization failed, cannot proceed with navigation." << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Get Navigation Status + Polling for SUCCESS/FAILED or Timeout (get_navigation_status_example)
examples/cpp/galbot_navigation/src/get_navigation_status_example.cpp
/**
* 示例：非阻塞导航中轮询 get_navigation_status，根据 SUCCESS/FAILED 或超时及时退出，
* 避免卡死并走错误逻辑。
*/
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>
using namespace galbot::sdk::g1;
int main() {
auto& navigation = GalbotNavigation::get_instance();
auto& robot = GalbotRobot::get_instance();
if (!robot.init()) {
std::cerr << "Base instance 初始化失败！" << std::endl;
return -1;
}
if (!navigation.init()) {
std::cerr << "Navigation instance 初始化失败！" << std::endl;
return -1;
}
auto res = robot.switch_controller(ControllerName::CHASSIS_POSE_CTRL);
if (res != ControlStatus::SUCCESS) {
std::cerr << "切换控制器失败！" << 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;
// 非阻塞导航
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 << "已到达目标" << std::endl;
break;
}
if (status == NavigationTaskStatus::FAILED) {
std::cout << "导航失败，及时退出错误逻辑" << std::endl;
break;
}
if (elapsed >= timeout_s) {
std::cout << "导航超时，及时退出" << std::endl;
break;
}
if (status == NavigationTaskStatus::RUNNING) {
std::cout << "正在导航... 状态: RUNNING, 已用时: " << elapsed << "s" << std::endl;
} else {
std::cout << "状态: 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 << "资源释放成功" << std::endl;
return 0;
}
Move Straight to Goal / Stop Navigation (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::g1;
// 辅助函数: 获取当前时间字符串 [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();
auto& robot = GalbotRobot::get_instance();
// 初始化系统
std::cout << get_timestamp() << "开始调用 robot.init()..." << std::endl;
if (robot.init()) {
std::cout << get_timestamp() << "Base instance 初始化成功！" << std::endl;
} else {
std::cerr << get_timestamp() << "Base instance 初始化失败！" << std::endl;
return -1;
}
std::cout << get_timestamp() << "开始调用 navigation.init()..." << std::endl;
if (navigation.init()) {
std::cout << get_timestamp() << "Navigation instance 初始化成功！" << std::endl;
} else {
std::cerr << get_timestamp() << "Navigation instance 初始化失败！" << std::endl;
return -1;
}
auto res = robot.switch_controller(ControllerName::CHASSIS_POSE_CTRL);
if (res != ControlStatus::SUCCESS) {
std::cerr << "切换控制器失败！" << 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});
// 检查重定位是否成功
std::cout << get_timestamp() << "进入重定位检查循环..." << std::endl;
int count_relocalize = 0;
while (!navigation.is_localized() && count_relocalize < 20) {
std::cout << get_timestamp() << "调用 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;
// 获取当前位姿
std::cout << get_timestamp() << "调用 navigation.get_current_pose()..." << std::endl;
Pose current_pose = navigation.get_current_pose();
std::cout << get_timestamp() << "当前位姿: 位置(" << 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 is_blocking = true;
// 最大等待到位时间
float timeout_s = 20;
// --- 直线移动并计时 ---
std::cout << "--------------------------------------------------" << std::endl;
std::cout << get_timestamp() << "准备执行 move_straight_to (直线移动)..." << std::endl;
// 记录开始时间
auto start_move = std::chrono::system_clock::now();
// 执行移动
NavigationStatus status = navigation.move_straight_to(goal_pose, is_blocking, timeout_s);
// 记录结束时间并计算耗时
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() << "已到达目标点位 (耗时: " << duration << "ms)" << std::endl;
} else {
std::cout << get_timestamp() << "导航失败，状态码: " << static_cast<int>(status)
<< " (耗时: " << duration << "ms)" << std::endl;
}
std::cout << "--------------------------------------------------" << std::endl;
// 停止导航
std::cout << get_timestamp() << "调用 navigation.stop_navigation()..." << std::endl;
navigation.stop_navigation();
// 再次获取当前位姿
std::cout << get_timestamp() << "调用 navigation.get_current_pose()..." << std::endl;
current_pose = navigation.get_current_pose();
std::cout << get_timestamp() << "当前位姿: 位置(" << 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;
} else {
std::cout << get_timestamp() << "重定位失败，无法继续执行导航。" << std::endl;
}
std::cout << get_timestamp() << "执行 shutdown..." << std::endl;
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
std::cout << get_timestamp() << "程序结束。" << std::endl;
return 0;
}
Complete Running Example (Simple Workflow) (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::g1;
int main() {
auto& navigation = GalbotNavigation::get_instance();
auto& robot = GalbotRobot::get_instance();
// 初始化系统
if (!robot.init()) {
std::cerr << "Base instance 初始化失败！" << std::endl;
return -1;
}
if (!navigation.init()) {
std::cerr << "Navigation instance 初始化失败！" << std::endl;
return -1;
}
auto res = robot.switch_controller(ControllerName::CHASSIS_POSE_CTRL);
if (res != ControlStatus::SUCCESS) {
std::cerr << "切换控制器失败！" << 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});
// 检查重定位是否成功
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 << "重定位成功！" << std::endl;
// 获取当前位姿
Pose current_pose = navigation.get_current_pose();
std::cout << "当前位姿: 位置(" << 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;
// 是否开启障碍物检查（环境空旷可设置为true）
bool enable_collision_check = false;
// 是否阻塞等待到达
bool is_blocking = true;
// 最大等待到位时间
float timeout_s = 20;
// 检查路径是否可达并导航到目标
if (navigation.check_path_reachability(goal_pose, init_pose)) {
std::cout << "路径可达，导航至目标点位" << std::endl;
// 导航到指定点位，关闭障碍物检查，阻塞等待到达，最大等待时间为20秒
NavigationStatus status = navigation.navigate_to_goal(
goal_pose, enable_collision_check, is_blocking, timeout_s);
if (status == NavigationStatus::SUCCESS) {
std::cout << "已到达目标点位" << std::endl;
} else {
std::cout << "导航失败，状态码: " << static_cast<int>(status) << std::endl;
}
} else {
std::cout << "路径不可达，无法导航至目标点位" << std::endl;
}
// 检查路径是否可达并回到起点
if (navigation.check_path_reachability(init_pose, goal_pose)) {
std::cout << "路径可达，导航至起点" << std::endl;
// 导航到指定点位，关闭障碍物检查，非阻塞等待到达
is_blocking = false;
NavigationStatus status = navigation.navigate_to_goal(
init_pose, enable_collision_check, is_blocking);
// 等待到达
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 << "已到达目标点位" << std::endl;
} else {
std::cout << "导航失败，未到达目标点位" << std::endl;
}
} else {
std::cout << "路径不可达，无法导航至起点" << std::endl;
}
// 检查路径是否可达并导航到目标
if (navigation.check_path_reachability(goal_pose, init_pose)) {
std::cout << "路径可达，导航至目标点位" << std::endl;
// 直线移动到目标点位,阻塞等待到达，最大等待时间为10秒
is_blocking = true;
NavigationStatus status = navigation.move_straight_to(
goal_pose, is_blocking, timeout_s);
if (status == NavigationStatus::SUCCESS) {
std::cout << "已到达目标点位" << std::endl;
} else {
std::cout << "导航失败，状态码: " << static_cast<int>(status) << std::endl;
}
} else {
std::cout << "路径不可达，无法导航至目标点位" << std::endl;
}
// 停止导航
navigation.stop_navigation();
} else {
std::cout << "重定位失败！" << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Class: GalbotPerception
Foundation Stereo Single Depth Inference (run_once + wait_for_new_result + get_latest_result)
Initialize the FOUNDATION_STEREO module, trigger one inference, wait for a new result, then colorize the depth map (instanceMask) and save it to an image file.
examples/cpp/galbot_perception/src/foundation_stereo_run_once_example.cpp
/**
* 高精度双目深度估计示例：使用 run_once + wait_for_new_result 获取单次推理结果
*/
#include <iostream>
#include <thread>
#include <chrono>
#include "galbot_robot.hpp"
#include "galbot_perception.hpp"
#include "opencv2/opencv.hpp"
using namespace galbot::sdk::g1;
int main() {
auto& robot = GalbotRobot::get_instance();
if (!robot.init()) {
std::cerr << "Robot 初始化失败" << std::endl;
return -1;
}
auto& perception = GalbotPerception::get_instance();
if (!perception.init({PerceptionModule::FOUNDATION_STEREO})) {
std::cerr << "感知模块初始化失败" << std::endl;
return -1;
}
// 等待感知模型load
std::this_thread::sleep_for(std::chrono::seconds(12));
std::cout << "初始化成功，发送单次推理请求..." << std::endl;
if (!perception.run_once(PerceptionModule::FOUNDATION_STEREO)) {
std::cerr << "run_once 命令发送失败" << std::endl;
return -1;
}
std::cout << "等待推理结果..." << std::endl;
if (!perception.wait_for_new_result(PerceptionModule::FOUNDATION_STEREO, 5.0)) {
std::cerr << "等待超时，未收到推理结果" << std::endl;
return -1;
}
DetectionResult result;
if (!perception.get_latest_result(PerceptionModule::FOUNDATION_STEREO, result)) {
std::cerr << "获取结果失败" << std::endl;
return -1;
}
std::cout << result.getResultInfo() << std::endl;
if (!result.instanceMask.empty()) {
cv::Mat depth_map = result.instanceMask;
std::cout << "深度图 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 << "深度值范围: [" << 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 << "深度图已保存至 foundation_stereo_depth.jpg" << std::endl;
} else {
std::cout << "未收到深度图 (instanceMask 为空)" << std::endl;
}
robot.request_shutdown();
robot.wait_for_shutdown();
robot.destroy();
return 0;
}
Class: Parameter
Create and Use 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::g1;
int main() {
// 通过构造函数创建 Parameter 并设置选项
auto p = std::make_shared<Parameter>();
p->setBlocking(true); // 设置是否阻塞执行
p->setCheckCollision(false); // 关闭碰撞检测
p->setTimeout(5.0); // 设置超时时间（秒）
p->setActuate("with_chain_only");// 设置驱动模式
p->setToolPose(false); // 是否考虑工具位姿
p->setReferenceFrame("base_link");
std::cout << "--- Parameter p ---" << std::endl;
std::cout << "blocking: " << (p->getBlocking() ? "True" : "False") << std::endl;
std::cout << "collision check: " << (p->getCheckCollision() ? "True" : "False") << std::endl;
std::cout << "timeout: " << p->getTimeout() << "s" << std::endl;
return 0;
}
Utility Functions
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::g1;
int main() {
auto& planner = GalbotMotion::get_instance();
MotionStatus status = MotionStatus::SUCCESS;
std::string status_str = planner.status_to_string(status);
std::cout << "MotionStatus 字符串: " << 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; // 打印类名
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;
}