fix(命名空间): 修复Unitree SDK类型命名空间问题并重构PoseData
- 将ChannelSubscriberPtr改为unitree::robot::ChannelSubscriberPtr - 重构PoseData结构为poseDate类,增加JSON序列化功能 - 更新相关代码以使用新的poseDate类
This commit is contained in:
@@ -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;
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user