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

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.

Browse Source
This commit is contained in:
Sucan126
2025-09-20 16:05:10 +08:00
parent 2de8f53ae2
commit 38db96e433
7 changed files with 462 additions and 195 deletions
Download Patch File Download Diff File Expand all files Collapse all files

172
src/custom_robot.cpp
View File

@@ -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>();
controller_->initialize();
navigation_ = std::make_unique<Navigation>();
navigation_->Init();
rsc_ = std::make_unique<unitree::robot::go2::RobotStateClient>();
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<std::string> 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<std::string> 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;
}