From 38db96e433d410fc074bd7c21cf5380f5b6f8f6a Mon Sep 17 00:00:00 2001 From: Sucan126 <632190820@qq.com> Date: Sat, 20 Sep 2025 16:05:10 +0800 Subject: [PATCH] Add navigation functionality to CustomRobot and Controller classes. Introduced Navigation class for handling mapping and pose navigation commands. Updated CustomRobot to process navigation commands and manage Navigation instance. Enhanced Controller with motion switching capabilities, improving overall robot control and navigation features. --- CMakeLists.txt | 1 + include/controller.hpp | 18 ++- include/custom_robot.hpp | 4 + include/navigation.hpp | 29 ++++ src/controller.cpp | 321 +++++++++++++++++---------------------- src/custom_robot.cpp | 172 +++++++++++++++++++-- src/navigation.cpp | 112 ++++++++++++++ 7 files changed, 462 insertions(+), 195 deletions(-) create mode 100644 include/navigation.hpp create mode 100644 src/navigation.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d69e634..8a5625d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,7 @@ set(SOURCES src/config.cpp src/logger.cpp src/mqtt.cpp + src/navigation.cpp ) # Create executable with simple name diff --git a/include/controller.hpp b/include/controller.hpp index 5f4fd29..b757025 100644 --- a/include/controller.hpp +++ b/include/controller.hpp @@ -6,10 +6,12 @@ #include #include +#include #include #include #include #include +#include "logger.hpp" namespace custom { @@ -44,11 +46,25 @@ public: bool UseRemoteCommandFromApi(bool isRemoteCommandsFromApi); bool MoveToAbsolutePosition(float x, float y, float yaw); bool MoveToIncrementPosition(float x, float y, float yaw); - + + // MotionSwitcher + bool CheckMode(std::string& form, std::string& name); + bool SelectMode(const std::string& nameOrAlias); + bool ReleaseMode(); private: + template + bool ExecuteSportCmd(Func&& func); + + template + bool ExecuteObstacleCmd(Func&& func); + + template + bool ExecuteMotionSwitchCmd(Func&& func); + std::unique_ptr sc_; std::unique_ptr oac_; + std::unique_ptr msc_; std::atomic running_; std::atomic initialized_; diff --git a/include/custom_robot.hpp b/include/custom_robot.hpp index 0e1f535..cf29fe6 100644 --- a/include/custom_robot.hpp +++ b/include/custom_robot.hpp @@ -4,6 +4,7 @@ #include "config.hpp" #include "logger.hpp" #include "mqtt.hpp" +#include "navigation.hpp" #include #include @@ -23,6 +24,7 @@ public: bool start(); Controller* getController() const { return controller_.get(); } + Navigation* getNavigation() const { return navigation_.get(); } bool GetServiceList(std::vector& serviceList); bool SwitchService(const std::string& serviceName, bool enable); @@ -37,11 +39,13 @@ public: bool processOacCmd(const std::string& cmd, const nlohmann::json& message); bool processSportCmd(const std::string& cmd, const nlohmann::json& message); bool processRscCmd(const std::string& cmd, const nlohmann::json& message); + bool processNavCmd(const std::string& cmd, const nlohmann::json& message); void printServiceList(const std::vector& serviceList, int filterStatus = -1); private: std::string generateRandomClientId() const; std::unique_ptr controller_; + std::unique_ptr navigation_; std::unique_ptr rsc_; std::unique_ptr mqttClient_; diff --git a/include/navigation.hpp b/include/navigation.hpp new file mode 100644 index 0000000..61a3f0b --- /dev/null +++ b/include/navigation.hpp @@ -0,0 +1,29 @@ +#ifndef NAVIGATION_HPP +#define NAVIGATION_HPP + +#include +#include +#include +#include "unitree/robot/client/client.hpp" + +class Navigation { +public: + Navigation(); + ~Navigation(); + + void Init(); + + 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(); + +private: + std::unique_ptr client_; + bool callSlamService(int api_id, const nlohmann::json& data); +}; + +#endif // NAVIGATION_HPP diff --git a/src/controller.cpp b/src/controller.cpp index b338d46..7ba4790 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -2,6 +2,9 @@ #include "config.hpp" #include "logger.hpp" #include +#include +#include +#include namespace custom { @@ -17,6 +20,7 @@ Controller::~Controller() { LOG_INFO("Shutting down robot controller"); sc_.reset(); oac_.reset(); + msc_.reset(); initialized_ = false; } @@ -30,6 +34,9 @@ bool Controller::initialize() { oac_->SetTimeout(3.0f); oac_->Init(); + msc_ = std::make_unique(); + msc_->SetTimeout(10.0f); + msc_->Init(); initialized_ = true; return true; @@ -67,236 +74,188 @@ bool Controller::stop() { return true; } +template +bool Controller::ExecuteSportCmd(Func&& func) { + if (!initialized_) { + LOG_ERROR("Controller not initialized"); + return false; + } + + if (!sc_) { + LOG_ERROR("SportClient not available"); + return false; + } + + try { + int32_t result = func(sc_.get()); + return result == 0; + } catch (const std::exception& e) { + LOG_ERROR("Command failed with exception: " + std::string(e.what())); + return false; + } catch (...) { + LOG_ERROR("Command failed with unknown exception"); + return false; + } +} +template +bool Controller::ExecuteObstacleCmd(Func&& func) { + if (!initialized_) { + LOG_ERROR("Controller not initialized"); + return false; + } + + if (!oac_) { + LOG_ERROR("ObstaclesAvoidClient not available"); + return false; + } + + try { + int32_t result = func(oac_.get()); + return result == 0; + } catch (const std::exception& e) { + LOG_ERROR("Command failed with exception: " + std::string(e.what())); + return false; + } catch (...) { + LOG_ERROR("Command failed with unknown exception"); + return false; + } +} + +template +bool Controller::ExecuteMotionSwitchCmd(Func&& func) { + if (!initialized_) { + LOG_ERROR("Controller not initialized"); + return false; + } + + if (!msc_) { + LOG_ERROR("MotionSwitcherClient not available"); + return false; + } + + try { + int32_t result = func(msc_.get()); + return result == 0; + } catch (const std::exception& e) { + LOG_ERROR("Command failed with exception: " + std::string(e.what())); + return false; + } catch (...) { + LOG_ERROR("Command failed with unknown exception"); + return false; + } +} bool Controller::StandUp() { - LOG_INFO("Standing up"); - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->StandUp(); - LOG_INFO("StandUp API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Stand up failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->StandUp(); + }); } bool Controller::StandDown() { - LOG_INFO("Standing down"); - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->StandDown(); - LOG_INFO("StandDown API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Stand down failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->StandDown(); + }); } bool Controller::Sit() { - LOG_INFO("Sitting"); - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->Sit(); - LOG_INFO("Sit API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Sit failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->Sit(); + }); } bool Controller::Damp() { - LOG_INFO("Damping"); - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->Damp(); - LOG_INFO("Damp API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Damp failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->Damp(); + }); } bool Controller::RecoveryStand() { - LOG_INFO("Executing recovery stand"); - - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->RecoveryStand(); - LOG_INFO("RecoveryStand API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Recovery stand failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->RecoveryStand(); + }); } bool Controller::StopMove() { - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->StopMove(); - LOG_INFO("StopMove API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Stop move failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->StopMove(); + }); } bool Controller::BalanceStand() { - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->BalanceStand(); - LOG_INFO("BalanceStand API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Balance stand failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->BalanceStand(); + }); } bool Controller::Dance1() { - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->Dance1(); - LOG_INFO("Dance1 API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Dance1 failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->Dance1(); + }); } bool Controller::Dance2() { - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->Dance2(); - LOG_INFO("Dance2 API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Dance2 failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->Dance2(); + }); } bool Controller::Hello() { - try { - if (!sc_) { - LOG_ERROR("SportClient not initialized"); - return false; - } - - int32_t result = sc_->Hello(); - LOG_INFO("Hello API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Hello failed: " + std::string(e.what())); - return false; - } + return ExecuteSportCmd([](auto* sc) { + return sc->Hello(); + }); } bool Controller::SwitchSet(bool enable) { - try { - if (!oac_) { - LOG_ERROR("ObstaclesAvoidClient not initialized"); - return false; - } - - int32_t result = oac_->SwitchSet(enable); - LOG_INFO("SwitchSet API returned: " + std::to_string(result)); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Switch set failed: " + std::string(e.what())); - return false; - } + return ExecuteObstacleCmd([enable](auto* oac) { + return oac->SwitchSet(enable); + }); } bool Controller::SwitchGet(bool& enable) { - try { - if (!oac_) { - LOG_ERROR("ObstaclesAvoidClient not initialized"); - return false; - } - - int32_t result = oac_->SwitchGet(enable); - LOG_INFO("SwitchGet API returned: " + std::to_string(result) + - ", enable: " + std::string(enable ? "true" : "false")); - return result == 0; - } catch (const std::exception& e) { - LOG_ERROR("Switch get failed: " + std::string(e.what())); - return false; - } + return ExecuteObstacleCmd([&enable](auto* oac) { + return oac->SwitchGet(enable); + }); } bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) { - try { - return oac_ && oac_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0; - } catch (const std::exception& e) { - LOG_ERROR("Use remote command from api failed: " + std::string(e.what())); - return false; - } + return ExecuteObstacleCmd([isRemoteCommandsFromApi](auto* oac) { + return oac->UseRemoteCommandFromApi(isRemoteCommandsFromApi); + }); } bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) { - try { - return oac_ && oac_->MoveToAbsolutePosition(x, y, yaw) == 0; - } catch (const std::exception& e) { - LOG_ERROR("Move to absolute position failed: " + std::string(e.what())); - return false; - } + return ExecuteObstacleCmd([x, y, yaw](auto* oac) { + return oac->MoveToAbsolutePosition(x, y, yaw); + }); } bool Controller::MoveToIncrementPosition(float x, float y, float yaw) { - try { - return oac_ && oac_->MoveToIncrementPosition(x, y, yaw) == 0; - } catch (const std::exception& e) { - LOG_ERROR("Move to increment position failed: " + std::string(e.what())); - return false; - } + return ExecuteObstacleCmd([x, y, yaw](auto* oac) { + return oac->MoveToIncrementPosition(x, y, yaw); + }); +} + +bool Controller::CheckMode(std::string& form, std::string& name) { + return ExecuteMotionSwitchCmd([&](auto* msc) { + return msc->CheckMode(form, name); + }); +} + +bool Controller::SelectMode(const std::string& nameOrAlias) { + return ExecuteMotionSwitchCmd([&](auto* msc) { + return msc->SelectMode(nameOrAlias); + }); +} + +bool Controller::ReleaseMode() { + return ExecuteMotionSwitchCmd([&](auto* msc) { + return msc->ReleaseMode(); + }); } } // namespace custom \ No newline at end of file diff --git a/src/custom_robot.cpp b/src/custom_robot.cpp index fa205a4..1923e26 100644 --- a/src/custom_robot.cpp +++ b/src/custom_robot.cpp @@ -12,7 +12,7 @@ namespace custom { CustomRobot::CustomRobot() - : initialized_(false), running_(false) { + : initialized_(false), running_(false), controller_(nullptr), navigation_(nullptr), rsc_(nullptr), mqttClient_(nullptr) { config_.loadDefaults(); try { @@ -41,6 +41,10 @@ CustomRobot::~CustomRobot() { if (rsc_) { rsc_.reset(); } + + if (navigation_) { + navigation_.reset(); + } try { unitree::robot::ChannelFactory::Instance()->Release(); @@ -57,6 +61,9 @@ bool CustomRobot::initialize() { controller_ = std::make_unique(); controller_->initialize(); + navigation_ = std::make_unique(); + navigation_->Init(); + rsc_ = std::make_unique(); rsc_->SetTimeout(3.0f); rsc_->Init(); @@ -244,6 +251,8 @@ void CustomRobot::processCmd(const nlohmann::json& message) { success = processSportCmd(cmd, message); } else if (type == "rsc") { success = processRscCmd(cmd, message); + } else if (type == "nav") { + success = processNavCmd(cmd, message); } else { LOG_ERROR("Unknown command type: " + type); return; @@ -330,6 +339,71 @@ bool CustomRobot::processOacCmd(const std::string& cmd, const nlohmann::json& me } } +bool CustomRobot::processNavCmd(const std::string& cmd, const nlohmann::json& message) { + if (!navigation_) { + LOG_ERROR("Navigation not initialized"); + return false; + } + + try { + if (cmd == "start_mapping") { + return navigation_->startMapping(); + } else if (cmd == "end_mapping") { + if (!message.contains("param") || !message["param"].contains("address")) { + LOG_ERROR("end_mapping cmd missing 'address' parameter"); + return false; + } + std::string address = message["param"]["address"]; + return navigation_->endMapping(address); + } else if (cmd == "initialize_pose") { + if (!message.contains("param")) { + LOG_ERROR("initialize_pose cmd missing 'param'"); + return false; + } + auto& param = message["param"]; + const std::vector keys = {"x", "y", "z", "q_x", "q_y", "q_z", "q_w", "address"}; + for (const auto& key : keys) { + if (!param.contains(key)) { + LOG_ERROR("initialize_pose missing parameter: " + key); + return false; + } + } + return navigation_->initializePose(param["x"], param["y"], param["z"], param["q_x"], param["q_y"], param["q_z"], param["q_w"], param["address"]); + } else if (cmd == "pose_navigation") { + if (!message.contains("param")) { + LOG_ERROR("pose_navigation cmd missing 'param'"); + return false; + } + auto& param = message["param"]; + if (!param.contains("targetPose") || !param.contains("mode") || !param.contains("speed")) { + LOG_ERROR("pose_navigation missing 'targetPose', 'mode', or 'speed'"); + return false; + } + auto& targetPose = param["targetPose"]; + const std::vector pose_keys = {"x", "y", "z", "q_x", "q_y", "q_z", "q_w"}; + for (const auto& key : pose_keys) { + if (!targetPose.contains(key)) { + LOG_ERROR("pose_navigation.targetPose missing parameter: " + key); + return false; + } + } + return navigation_->poseNavigation(targetPose["x"], targetPose["y"], targetPose["z"], targetPose["q_x"], targetPose["q_y"], targetPose["q_z"], targetPose["q_w"], param["mode"], param["speed"]); + } else if (cmd == "pause_navigation") { + return navigation_->pauseNavigation(); + } else if (cmd == "resume_navigation") { + return navigation_->resumeNavigation(); + } else if (cmd == "close_slam") { + return navigation_->closeSlam(); + } else { + LOG_ERROR("Unknown Nav command: " + cmd); + return false; + } + } catch (const std::exception& e) { + LOG_ERROR("Error executing Nav command " + cmd + ": " + std::string(e.what())); + return false; + } +} + bool CustomRobot::processRscCmd(const std::string& cmd, const nlohmann::json& message) { try { if (cmd == "list") { @@ -417,27 +491,99 @@ bool CustomRobot::processSportCmd(const std::string& cmd, const nlohmann::json& } try { - if (cmd == "StandUp") { + if (cmd == "Damp") { + return controller_->Damp(); + } else if (cmd == "BalanceStand") { + return controller_->BalanceStand(); + } else if (cmd == "StopMove") { + return controller_->StopMove(); + } else if (cmd == "StandUp") { return controller_->StandUp(); } else if (cmd == "StandDown") { return controller_->StandDown(); - } else if (cmd == "Sit") { - return controller_->Sit(); - } else if (cmd == "Damp") { - return controller_->Damp(); } else if (cmd == "RecoveryStand") { return controller_->RecoveryStand(); - } else if (cmd == "StopMove") { - return controller_->StopMove(); - } else if (cmd == "BalanceStand") { - return controller_->BalanceStand(); + } else if (cmd == "Euler") { + return controller_->Euler(message["param"]["roll"], message["param"]["pitch"], message["param"]["yaw"]); + } else if (cmd == "Move") { + return controller_->Move(message["param"]["vx"], message["param"]["vy"], message["param"]["vyaw"]); + } else if (cmd == "Sit") { + return controller_->Sit(); + } else if (cmd == "RiseSit") { + return controller_->RiseSit(); + } else if (cmd == "SpeedLevel") { + return controller_->SpeedLevel(message["param"]["level"]); + } else if (cmd == "Hello") { + return controller_->Hello(); + } else if (cmd == "Stretch") { + return controller_->Stretch(); + } else if (cmd == "SwitchJoystick") { + return controller_->SwitchJoystick(message["param"]["flag"]); + } else if (cmd == "Content") { + return controller_->Content(); + } else if (cmd == "Heart") { + return controller_->Heart(); + } else if (cmd == "Pose") { + return controller_->Pose(message["param"]["flag"]); + } else if (cmd == "Scrape") { + return controller_->Scrape(); + } else if (cmd == "FrontFlip") { + return controller_->FrontFlip(); + } else if (cmd == "FrontJump") { + return controller_->FrontJump(); + } else if (cmd == "FrontPounce") { + return controller_->FrontPounce(); } else if (cmd == "Dance1") { return controller_->Dance1(); } else if (cmd == "Dance2") { return controller_->Dance2(); - } else if (cmd == "Hello") { - return controller_->Hello(); - } else { + } else if (cmd == "LeftFlip") { + return controller_->LeftFlip(); + } else if (cmd == "BackFlip") { + return controller_->BackFlip(); + } else if (cmd == "HandStand") { + return controller_->HandStand(message["param"]["flag"]); + } else if (cmd == "FreeWalk") { + return controller_->FreeWalk(); + } else if (cmd == "FreeBound") { + return controller_->FreeBound(message["param"]["flag"]); + } else if (cmd == "FreeJump") { + return controller_->FreeJump(message["param"]["flag"]); + } else if (cmd == "FreeAvoid") { + return controller_->FreeAvoid(message["param"]["flag"]); + } else if (cmd == "ClassicWalk") { + return controller_->ClassicWalk(message["param"]["flag"]); + } else if (cmd == "WalkUpright") { + return controller_->WalkUpright(message["param"]["flag"]); + } else if (cmd == "CrossStep") { + return controller_->CrossStep(message["param"]["flag"]); + } else if (cmd == "AutoRecoverSet") { + return controller_->AutoRecoverSet(message["param"]["flag"]); + } else if (cmd == "StaticWalk") { + return controller_->StaticWalk(); + } else if (cmd == "TrotRun") { + return controller_->TrotRun(); + } else if (cmd == "EconomicGait") { + return controller_->EconomicGait(); + } else if (cmd == "SwitchAvoidMode") { + return controller_->SwitchAvoidMode(); + } + else if (cmd == "BodyHeight") { + return controller_->BodyHeight(message["param"]["height"]); + } else if (cmd == "SwitchGait") { + return controller_->SwitchGait(message["param"]["gait"]); + } else if (cmd == "TrajectoryFollow") { + return controller_->TrajectoryFollow(message["param"]["path"]); + } else if (cmd == "ContinuousGait") { + return controller_->ContinuousGait(message["param"]["flag"]); + } else if (cmd == "MoveToPos") { + return controller_->MoveToPos(message["param"]["x"], message["param"]["y"], message["param"]["yaw"]); + } else if (cmd == "FastWalk") { + return controller_->FastWalk(message["param"]["flag"]); + } else if (cmd == "FootRaiseHeight") { + return controller_->FootRaiseHeight(message["param"]["height"]); + } + else { LOG_ERROR("Unknown Sport command: " + cmd); return false; } diff --git a/src/navigation.cpp b/src/navigation.cpp new file mode 100644 index 0000000..9c52702 --- /dev/null +++ b/src/navigation.cpp @@ -0,0 +1,112 @@ +#include "navigation.hpp" +#include "logger.hpp" +#include "unitree/robot/channel/channel_factory.hpp" +#include + +const std::string SLAM_SERVICE_NAME = "slam_operate"; + +Navigation::Navigation() : client_(nullptr) {} + +Navigation::~Navigation() {} + +void Navigation::Init() { + try { + client_ = std::make_unique(SLAM_SERVICE_NAME); + client_->Init(); + LOG_INFO("Navigation client for service '{}' initialized.", SLAM_SERVICE_NAME); + } catch (const std::exception& e) { + LOG_ERROR("Failed to initialize navigation client: {}", e.what()); + } +} + +bool Navigation::callSlamService(int api_id, const nlohmann::json& data) { + if (!client_) { + LOG_ERROR("Navigation client not initialized."); + return false; + } + + nlohmann::json request_json; + request_json["data"] = data; + std::string request_str = request_json.dump(); + std::string response_str; + + LOG_INFO("Calling slam service api_id: {}, request: {}", api_id, request_str); + + int ret = client_->Call(api_id, request_str, response_str); + + if (ret != 0) { + LOG_ERROR("Slam service request failed with SDK error code: {}", ret); + return false; + } + + LOG_INFO("Slam service response: {}", response_str); + + try { + nlohmann::json response_json = nlohmann::json::parse(response_str); + if (response_json.contains("succeed") && response_json["succeed"].get()) { + LOG_INFO("Slam service call for api_id {} succeeded. Info: {}", api_id, response_json.value("info", "")); + return true; + } else { + LOG_ERROR("Slam service call for api_id {} failed. Error code: {}, Info: {}", + api_id, response_json.value("errorCode", -1), response_json.value("info", "")); + return false; + } + } catch (const nlohmann::json::parse_error& e) { + LOG_ERROR("Failed to parse slam service response: {}", e.what()); + return false; + } +} + +bool Navigation::startMapping() { + nlohmann::json data; + data["slam_type"] = "indoor"; + return callSlamService(1801, data); +} + +bool Navigation::endMapping(const std::string& address) { + nlohmann::json data; + data["address"] = address; + return callSlamService(1802, 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) { + nlohmann::json data; + data["x"] = x; + data["y"] = y; + data["z"] = z; + data["q_x"] = q_x; + data["q_y"] = q_y; + data["q_z"] = q_z; + data["q_w"] = q_w; + data["address"] = address; + return callSlamService(1804, 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) { + nlohmann::json data; + data["targetPose"]["x"] = x; + data["targetPose"]["y"] = y; + data["targetPose"]["z"] = z; + data["targetPose"]["q_x"] = q_x; + data["targetPose"]["q_y"] = q_y; + data["targetPose"]["q_z"] = q_z; + data["targetPose"]["q_w"] = q_w; + data["mode"] = mode; + data["speed"] = speed; + return callSlamService(1102, data); +} + +bool Navigation::pauseNavigation() { + nlohmann::json data; + return callSlamService(1201, data); +} + +bool Navigation::resumeNavigation() { + nlohmann::json data; + return callSlamService(1202, data); +} + +bool Navigation::closeSlam() { + nlohmann::json data; + return callSlamService(1901, data); +}