fix(命名空间): 修复Unitree SDK类型命名空间问题并重构PoseData

- 将ChannelSubscriberPtr改为unitree::robot::ChannelSubscriberPtr
- 重构PoseData结构为poseDate类,增加JSON序列化功能
- 更新相关代码以使用新的poseDate类
This commit is contained in:
2025-09-22 14:46:18 +08:00
parent ccaf5f3513
commit 6b9ad80757
3 changed files with 45 additions and 17 deletions

View File

@@ -26,6 +26,39 @@ const int32_t ROBOT_API_ID_RESUME_NAV = 1202;
namespace custom {
class poseDate
{
public:
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
float q_x = 0.0f;
float q_y = 0.0f;
float q_z = 0.0f;
float q_w = 1.0f;
int mode = 1;
float speed = 0.8f;
std::string toJsonStr() const
{
nlohmann::json j;
j["data"]["targetPose"]["x"] = x;
j["data"]["targetPose"]["y"] = y;
j["data"]["targetPose"]["z"] = z;
j["data"]["targetPose"]["q_x"] = q_x;
j["data"]["targetPose"]["q_y"] = q_y;
j["data"]["targetPose"]["q_z"] = q_z;
j["data"]["targetPose"]["q_w"] = q_w;
j["data"]["mode"] = mode;
j["data"]["speed"] = speed;
return j.dump(4);
}
void printInfo() const
{
std::cout << "x:" << x << " y:" << y << " z:" << z << " q_x:"
<< q_x << " q_y:" << q_y << " q_z:" << q_z << " q_w:" << q_w << std::endl;
}
};
class Navigation : public unitree::robot::Client {
public:
Navigation();
@@ -41,7 +74,7 @@ public:
bool resumeNavigation();
bool closeSlam();
PoseData getCurrentPose() const { return currentPose; }
poseDate getCurrentPose() const { return currentPose; }
bool hasArrived() const { return is_arrived; }
@@ -50,21 +83,10 @@ private:
void slamInfoHandler(const void *message);
void slamKeyInfoHandler(const void *message);
ChannelSubscriberPtr subSlamInfo;
ChannelSubscriberPtr subSlamKeyInfo;
unitree::robot::ChannelSubscriberPtr subSlamInfo;
unitree::robot::ChannelSubscriberPtr subSlamKeyInfo;
// Structure to hold pose data
struct PoseData {
double x = 0.0;
double y = 0.0;
double z = 0.0;
double q_x = 0.0;
double q_y = 0.0;
double q_z = 0.0;
double q_w = 0.0;
};
PoseData currentPose;
poseDate currentPose;
bool is_arrived = false;
};