tanjianbin/lzwc-terminal-unitreeGo2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(导航): 实现SLAM信息处理和导航状态管理

Browse Source 
添加SLAM信息订阅处理功能，包括位姿更新和任务结果处理
引入PoseData结构体存储当前位姿信息
使用常量定义替换硬编码的API ID
增加导航状态管理功能(到达检测)
This commit is contained in:
Sucan126
2025-09-22 11:33:15 +08:00
parent 1d8daa7542
commit ec6f4c0057
2 changed files with 122 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
include/navigation.hpp
View File

@@ -1,10 +1,28 @@
#ifndef NAVIGATION_HPP
#define NAVIGATION_HPP
#ifndef __NAVIGATION_HPP__
#define __NAVIGATION_HPP__
#include <string>
#include <memory>
#include <unitree/robot/client/client.hpp>
#include <nlohmann/json.hpp>
#include "unitree/robot/client/client.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 {
@@ -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__

89
src/navigation.cpp
View File

@@ -1,21 +1,32 @@
#include "navigation.hpp"
#include "logger.hpp"
#include "unitree/robot/channel/channel_factory.hpp"
#include <iostream>
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<std_msgs::msg::dds_::String_>(SlamInfoTopic));
subSlamInfo->InitChannel(
std::bind(&Navigation::slamInfoHandler, this, std::placeholders::_1), 1);
subSlamKeyInfo = ChannelSubscriberPtr(
new unitree::robot::ChannelSubscriber<std_msgs::msg::dds_::String_>(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