Sucan126
ec6f4c0057
添加SLAM信息订阅处理功能，包括位姿更新和任务结果处理 引入PoseData结构体存储当前位姿信息 使用常量定义替换硬编码的API ID 增加导航状态管理功能(到达检测)
74 lines
2.0 KiB
C++
74 lines
2.0 KiB
C++
#ifndef __NAVIGATION_HPP__
|
|
#define __NAVIGATION_HPP__
|
|
|
|
#include <unitree/robot/client/client.hpp>
|
|
#include <nlohmann/json.hpp>
|
|
#include <string>
|
|
#include <iostream>
|
|
|
|
#include "std_msgs/msg/dds_/String_.hpp"
|
|
#include "unitree/robot/channel/channel_subscriber.hpp"
|
|
|
|
|
|
const std::string SLAM_SERVICE_NAME = "slam_operate";
|
|
const std::string SLAM_API_VERSION = "1.0.0";
|
|
const int32_t ROBOT_API_ID_STOP_NODE = 1901;
|
|
const int32_t ROBOT_API_ID_START_MAPPING_PL = 1801;
|
|
const int32_t ROBOT_API_ID_END_MAPPING_PL = 1802;
|
|
const int32_t ROBOT_API_ID_START_RELOCATION_PL = 1804;
|
|
const int32_t ROBOT_API_ID_POSE_NAV_PL = 1102;
|
|
const int32_t ROBOT_API_ID_PAUSE_NAV = 1201;
|
|
const int32_t ROBOT_API_ID_RESUME_NAV = 1202;
|
|
|
|
// DDS topic names
|
|
#define SlamInfoTopic "rt/slam_info"
|
|
#define SlamKeyInfoTopic "rt/slam_key_info"
|
|
|
|
namespace custom {
|
|
|
|
class Navigation : public unitree::robot::Client {
|
|
public:
|
|
Navigation();
|
|
~Navigation();
|
|
|
|
void Init() override;
|
|
|
|
bool startMapping();
|
|
bool endMapping(const std::string& address);
|
|
bool initializePose(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, const std::string& address);
|
|
bool poseNavigation(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, int mode, double speed);
|
|
bool pauseNavigation();
|
|
bool resumeNavigation();
|
|
bool closeSlam();
|
|
|
|
PoseData getCurrentPose() const { return currentPose; }
|
|
|
|
bool hasArrived() const { return is_arrived; }
|
|
|
|
private:
|
|
bool callSlamService(int api_id, const nlohmann::json& data);
|
|
void slamInfoHandler(const void *message);
|
|
void slamKeyInfoHandler(const void *message);
|
|
|
|
ChannelSubscriberPtr subSlamInfo;
|
|
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;
|
|
bool is_arrived = false;
|
|
};
|
|
|
|
} // namespace custom
|
|
|
|
#endif // __NAVIGATION_HPP__
|