diff --git a/include/navigation.hpp b/include/navigation.hpp index feb5f6f..ff47f77 100644 --- a/include/navigation.hpp +++ b/include/navigation.hpp @@ -1,10 +1,28 @@ -#ifndef NAVIGATION_HPP -#define NAVIGATION_HPP +#ifndef __NAVIGATION_HPP__ +#define __NAVIGATION_HPP__ -#include -#include +#include #include -#include "unitree/robot/client/client.hpp" +#include +#include + +#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 { @@ -13,7 +31,7 @@ public: Navigation(); ~Navigation(); - void Init(); + void Init() override; bool startMapping(); bool endMapping(const std::string& address); @@ -23,10 +41,33 @@ public: 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 +#endif // __NAVIGATION_HPP__ diff --git a/src/navigation.cpp b/src/navigation.cpp index f2ca482..8ff1e1b 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -1,21 +1,32 @@ #include "navigation.hpp" #include "logger.hpp" -#include "unitree/robot/channel/channel_factory.hpp" -#include - -const std::string SLAM_SERVICE_NAME = "slam_operate"; namespace custom { -Navigation::Navigation() : unitree::robot::Client(SLAM_SERVICE_NAME) {} +Navigation::Navigation() : unitree::robot::Client(SLAM_SERVICE_NAME) { + subSlamInfo = ChannelSubscriberPtr( + new unitree::robot::ChannelSubscriber(SlamInfoTopic)); + subSlamInfo->InitChannel( + std::bind(&Navigation::slamInfoHandler, this, std::placeholders::_1), 1); + + subSlamKeyInfo = ChannelSubscriberPtr( + new unitree::robot::ChannelSubscriber(SlamKeyInfoTopic)); + subSlamKeyInfo->InitChannel( + std::bind(&Navigation::slamKeyInfoHandler, this, std::placeholders::_1), 1); +} Navigation::~Navigation() {} void Navigation::Init() { try { - // Client::Init() is a pure virtual function in ClientBase and must be implemented - // by derived classes. Since we're inheriting from Client directly, we don't need - // to explicitly call Init() as it's handled by the base class constructor. + SetApiVersion(SLAM_API_VERSION); + UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_POSE_NAV_PL); + UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_PAUSE_NAV); + UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_RESUME_NAV); + UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_STOP_NODE); + UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_START_MAPPING_PL); + UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_END_MAPPING_PL); + UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_START_RELOCATION_PL); LOG_INFO("Navigation client for service '" + SLAM_SERVICE_NAME + "' initialized."); } catch (const std::exception& e) { LOG_ERROR("Failed to initialize navigation client: " + std::string(e.what())); @@ -55,16 +66,64 @@ bool Navigation::callSlamService(int api_id, const nlohmann::json& data) { } } +void Navigation::slamInfoHandler(const void *message) { + std_msgs::msg::dds_::String_ currentMsg = *(std_msgs::msg::dds_::String_ *)message; + nlohmann::json jsonData = nlohmann::json::parse(currentMsg.data()); + + // Check for errors + if (jsonData["errorCode"] != 0) { + LOG_WARN("SLAM info error: " + jsonData.value("info", "")); + return; + } + + // Process pose information + if (jsonData["type"] == "pos_info") { + currentPose.x = jsonData["data"]["currentPose"]["x"]; + currentPose.y = jsonData["data"]["currentPose"]["y"]; + currentPose.z = jsonData["data"]["currentPose"]["z"]; + currentPose.q_x = jsonData["data"]["currentPose"]["q_x"]; + currentPose.q_y = jsonData["data"]["currentPose"]["q_y"]; + currentPose.q_z = jsonData["data"]["currentPose"]["q_z"]; + currentPose.q_w = jsonData["data"]["currentPose"]["q_w"]; + LOG_INFO("Current pose updated: x=" + std::to_string(currentPose.x) + + ", y=" + std::to_string(currentPose.y) + + ", z=" + std::to_string(currentPose.z)); + } +} + +void Navigation::slamKeyInfoHandler(const void *message) { + std_msgs::msg::dds_::String_ currentMsg = *(std_msgs::msg::dds_::String_ *)message; + nlohmann::json jsonData = nlohmann::json::parse(currentMsg.data()); + + // Check for errors + if (jsonData["errorCode"] != 0) { + LOG_WARN("SLAM key info error: " + jsonData.value("info", "")); + return; + } + + // Process task results + if (jsonData["type"] == "task_result") { + is_arrived = jsonData["data"]["is_arrived"]; + std::string targetNodeName = jsonData["data"]["targetNodeName"]; + if (is_arrived) { + LOG_INFO("Arrived at target: " + targetNodeName); + } else { + LOG_WARN("Failed to reach target: " + targetNodeName + ". Please help!"); + } + } +} + + bool Navigation::startMapping() { nlohmann::json data; data["slam_type"] = "indoor"; - return callSlamService(1801, data); + return callSlamService(ROBOT_API_ID_START_MAPPING_PL, data); } bool Navigation::endMapping(const std::string& address) { nlohmann::json data; data["address"] = address; - return callSlamService(1802, data); + return callSlamService(ROBOT_API_ID_END_MAPPING_PL, data); } bool Navigation::initializePose(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, const std::string& address) { @@ -77,7 +136,7 @@ bool Navigation::initializePose(double x, double y, double z, double q_x, double data["q_z"] = q_z; data["q_w"] = q_w; data["address"] = address; - return callSlamService(1804, data); + return callSlamService(ROBOT_API_ID_START_RELOCATION_PL, data); } bool Navigation::poseNavigation(double x, double y, double z, double q_x, double q_y, double q_z, double q_w, int mode, double speed) { @@ -91,22 +150,22 @@ bool Navigation::poseNavigation(double x, double y, double z, double q_x, double data["targetPose"]["q_w"] = q_w; data["mode"] = mode; data["speed"] = speed; - return callSlamService(1102, data); + return callSlamService(ROBOT_API_ID_POSE_NAV_PL, data); } bool Navigation::pauseNavigation() { nlohmann::json data; - return callSlamService(1201, data); + return callSlamService(ROBOT_API_ID_PAUSE_NAV, data); } bool Navigation::resumeNavigation() { nlohmann::json data; - return callSlamService(1202, data); + return callSlamService(ROBOT_API_ID_RESUME_NAV, data); } bool Navigation::closeSlam() { nlohmann::json data; - return callSlamService(1901, data); + return callSlamService(ROBOT_API_ID_STOP_NODE, data); } } // namespace custom