Sucan126
6b9ad80757
- 将ChannelSubscriberPtr改为unitree::robot::ChannelSubscriberPtr - 重构PoseData结构为poseDate类，增加JSON序列化功能 - 更新相关代码以使用新的poseDate类
96 lines
2.7 KiB
C++
96 lines
2.7 KiB
C++
#ifndef __NAVIGATION_HPP__
|
|
#define __NAVIGATION_HPP__
|
|
|
|
#include <unitree/robot/client/client.hpp>
|
|
#include <nlohmann/json.hpp>
|
|
#include <string>
|
|
#include <iostream>
|
|
|
|
#include <unitree/idl/ros2/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 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();
|
|
~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();
|
|
|
|
poseDate 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);
|
|
|
|
unitree::robot::ChannelSubscriberPtr subSlamInfo;
|
|
unitree::robot::ChannelSubscriberPtr subSlamKeyInfo;
|
|
|
|
poseDate currentPose;
|
|
bool is_arrived = false;
|
|
};
|
|
|
|
} // namespace custom
|
|
|
|
#endif // __NAVIGATION_HPP__
|