meswork/smart_wasm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

按功能拆分接口并清理核心乱码注释

Browse Source
This commit is contained in:
zhangshun
2026-06-01 17:59:51 +08:00
parent 12310de782
commit 6aa98241ef
12 changed files with 946 additions and 958 deletions
Download Patch File Download Diff File Expand all files Collapse all files

609
src/KinematicsWebAPI.cpp
View File

@@ -1,609 +0,0 @@
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
#include "KinematicsWebAPI.h"
#include "FourBarMechanism/CrankSliderMechanism.h"
#include "URDFStrings.h"
#include <iostream>
#include <thread>
#include <chrono>
#include <iomanip>
#include <sstream>
#include <codecvt>
#include <locale>
#include <csignal>
// inversePoseStr2PSteps
KinematicsWebAPI::KinematicsWebAPI()
{
init_func();
}
std::string KinematicsWebAPI::init_func()
{
std::string response_string;
std::string urdf_string = URDFStrings::abb120_urdf; // 使用新的命名空间访问
auto result = RobotManager::initRobot(urdf_string, "9D7EAEF4-1AAB-499E-8783-B6CE016BC6D1");
auto result1 = RobotManager::initRobot(urdf_string, "abb_irb120_3_58");
json error_response = utils::create_api_response(true, 200, "init_func", "", "", "");
response_string = error_response.dump();
return response_string;
}
std::string KinematicsWebAPI::func(std::string sanitized_body)
{
// 添加调试输出
log("func called with body: " + sanitized_body.substr(0, 100));
assert(!sanitized_body.empty() && "sanitized_body should not be empty");
std::string response_string;
try
{
json request_json;
// 构建响应数据
json result;
try
{
request_json = json::parse(sanitized_body);
}
catch (const std::exception &e)
{
json error_response = utils::create_api_response(false, 400, "Invalid JSON format", "", "", "");
response_string = error_response.dump();
return response_string;
}
std::string msg = request_json.value("msg", "");
std::string req_code = request_json.value("req_code", "");
std::string req_from = request_json.value("req_from", "");
std::string req_cmd = request_json.value("req_cmd", "");
json req_param = request_json.value("req_param", json::object());
json res_data;
// 使用if语句直接处理对应的命令，不通过中间函数
if (req_cmd == "Cmd_Kinematics_inverse_pose_str")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
result = robot->inversePoseStr(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str_2PSteps command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
std::string steps_str = req_param.value("steps_str", "default");
int steps = std::stoi(steps_str);
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
result = robot->inversePoseStr2PSteps(pose_str, q_init_str, steps);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_strNoDifference command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
result = robot->inversePoseStrNoDifference(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_pose_str")
{
try
{
log("Handling Cmd_Kinematics_forward_pose_str command");
std::string joints_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
auto joints = robot->parseJointString(joints_str);
if (joints.size() != 6)
{
res_data = {{"error", "Invalid joints format"}};
}
else
{
double tcp_pose[7];
if (robot->calculateFK_TCP(joints.data(), tcp_pose))
{
res_data = {
{"position", {tcp_pose[0], tcp_pose[1], tcp_pose[2]}},
{"orientation", {tcp_pose[3], tcp_pose[4], tcp_pose[5], tcp_pose[6]}},
{"joints", joints},
{"success", true}};
}
else
{
res_data = {{"error", "Forward kinematics calculation failed"}};
}
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_SelectCraftTree")
{
try
{
log("Handling Cmd_SelectCraftTree command");
std::string tree_id = req_param.value("tree_id", "");
res_data = {
{"success", true},
{"tree_id", tree_id},
{"message", "Craft tree selected successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to select craft tree: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_AddOperationTree")
{
try
{
log("Handling Cmd_AddOperationTree command");
std::string tree_name = req_param.value("name", "");
json operations = req_param.value("operations", json::array());
res_data = {
{"success", true},
{"tree_name", tree_name},
{"operations_count", operations.size()},
{"message", "Operation tree added successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to add operation tree: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_InitRobot")
{
try
{
log("Handling Cmd_InitRobot command");
std::string urdf_base64 = req_param.value("urdf_base64", "");
std::string uuid = req_param.value("robot_uuid", "");
bool force_update = req_param.value("force_update", true);
std::string urdf_content = utils::base64_to_urdf(urdf_base64);
if (!utils::validate_urdf_base64(urdf_base64))
{
res_data = {
{"success", false},
{"message", "Invalid URDF format"},
{"timestamp", utils::get_current_time()}};
}
else
{
auto result = RobotManager::initRobot(urdf_content, uuid, force_update);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to initialize robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_GetRobot")
{
try
{
log("Handling Cmd_GetRobot command");
std::string uuid = req_param.value("uuid", "");
auto robot = RobotManager::getRobot(uuid);
if (!robot)
{
res_data = {{"error", "Robot not found"}};
}
else
{
res_data = {
{"success", true},
{"uuid", uuid},
{"initialized", robot->isInitialized()},
{"joints_count", robot->getNumberOfJoints()},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to get robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_RemoveRobot")
{
try
{
log("Handling Cmd_RemoveRobot command");
std::string uuid = req_param.value("uuid", "");
auto result = RobotManager::removeRobot(uuid);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to remove robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_ListRobots")
{
try
{
log("Handling Cmd_ListRobots command");
bool detail = req_param.value("detail", false);
auto robots = RobotManager::listRobots(detail);
res_data = {
{"success", true},
{"robots", robots},
{"count", robots.size()},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to list robots: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_all_joints")
{
try
{
log("Handling Cmd_Kinematics_forward_all_joints command");
std::string joints_str = req_param.value("joints_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
auto joints_poses_array = robot->handleKinematicsForwardAllJoints(joints_str);
res_data = {
{"OPERATION", joints_poses_array},
{"success", true}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Spc")
{
try
{
log("Handling " + req_cmd + " command");
json result = SpcCalculator::Spc(req_param);
res_data = result;
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to Cmd_Spc: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_FourBar_CrankSlider")
{
// 请求参数示例:
// {
// "L_AB": 0.5,
// "L_BS": 2.0,
// "S_OFS": 0.0,
// "angleDeg": 45.0
// }
try
{
log("Handling " + req_cmd + " command");
// 从请求参数中提取值
double L_AB = req_param.value("L_AB", 0.5);
double L_BS = req_param.value("L_BS", 2.0);
double S_OFS = req_param.value("S_OFS", 0.0);
double angleDeg = req_param.value("angleDeg", 0.0);
log("Parameters: L_AB=" + std::to_string(L_AB) +
", L_BS=" + std::to_string(L_BS) +
", S_OFS=" + std::to_string(S_OFS) +
", angleDeg=" + std::to_string(angleDeg));
// 创建曲柄滑块机构实例
std::unique_ptr<CrankSliderMechanism> mechanism(createCrankSliderMechanism());
// 设置连杆参数
mechanism->setL_AB(L_AB);
mechanism->setL_BS(L_BS);
mechanism->setS_OFS(S_OFS);
// 验证参数
ValidationResult validation = mechanism->validateParameters();
if (!validation.isValid())
{
res_data = {
{"success", false},
{"error", "Invalid parameters"},
{"validation_errors", validation.Errors},
{"validation_warnings", validation.Warnings}};
}
else
{
// 执行计算
MechanismState state = mechanism->calculate(angleDeg);
// 检查是否有错误
if (state.hasError())
{
res_data = {
{"success", false},
{"error", state.ErrorMessage}};
}
else
{
// 构建响应数据
json result;
result["success"] = true;
// 添加点坐标
json points_json;
for (const auto &point_pair : state.Points)
{
json point;
point["x"] = point_pair.second.X;
point["y"] = point_pair.second.Y;
points_json[point_pair.first] = point;
}
result["points"] = points_json;
// 添加姿态信息
json poses_json;
for (const auto &pose_pair : state.Poses)
{
json pose;
pose["tx"] = pose_pair.second.tx;
pose["ty"] = pose_pair.second.ty;
pose["tz"] = pose_pair.second.tz;
pose["qx"] = pose_pair.second.qx;
pose["qy"] = pose_pair.second.qy;
pose["qz"] = pose_pair.second.qz;
pose["qw"] = pose_pair.second.qw;
poses_json[pose_pair.first] = pose;
}
result["poses"] = poses_json;
// 添加角度信息
json angles_json;
for (const auto &angle_pair : state.Angles)
{
angles_json[angle_pair.first] = angle_pair.second;
}
result["angles"] = angles_json;
// 添加输入值
result["input_value"] = state.InputValue;
// 添加警告信息（如果有）
if (state.hasWarning())
{
result["warning"] = state.WarningMessage;
}
// 添加轨迹信息
std::vector<Vector2D> trajectory = mechanism->getTrajectoryPoints();
std::vector<Vector2D> slider_trajectory = mechanism->getSliderTrajectory();
json trajectory_json;
for (size_t i = 0; i < trajectory.size(); i++)
{
json point;
point["x"] = trajectory[i].X;
point["y"] = trajectory[i].Y;
trajectory_json.push_back(point);
}
result["trajectory"] = trajectory_json;
json slider_trajectory_json;
for (size_t i = 0; i < slider_trajectory.size(); i++)
{
json point;
point["x"] = slider_trajectory[i].X;
point["y"] = slider_trajectory[i].Y;
slider_trajectory_json.push_back(point);
}
result["slider_trajectory"] = slider_trajectory_json;
// 添加参数信息
result["parameters"] = {
{"L_AB", L_AB},
{"L_BS", L_BS},
{"S_OFS", S_OFS},
{"angleDeg", angleDeg}};
res_data = result;
}
}
}
catch (const std::exception &e)
{
res_data = {
{"success", false},
{"error", "Failed to Cmd_FourBar_CrankSlider: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles")
{
// json j = json::parse(jsonStr);
std::cout << "[DEBUG 0001] Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles 步骤1: 初始化数据结构" << std::endl;
std::string jsonInput = req_param.dump();
result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(jsonInput);
// if (req_param.empty())
// {
// result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles();
// }
// else
// {
// std::string jsonInput = req_param.dump();
// result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(jsonInput);
// }
res_data = result;
}
else if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
{
std::cout << "[DEBUG] Cmd_QuadrupedRobot_PerformForwardKinematics = " << std::endl;
std::string jsonInput = req_param.dump();
result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(jsonInput);
// if (req_param.empty())
// {
// result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics();
// }
// else
// {
// std::string jsonInput = req_param.dump();
// result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(jsonInput);
// }
res_data = result;
}
else
{
// 未知命令，调用默认处理函数
res_data["success"] = false;
res_data["error"] = "Unknown command: " + req_cmd;
res_data["received_params"] = req_param;
res_data["timestamp"] = std::time(nullptr);
}
json response = utils::create_api_response(true, 0, msg, req_code, req_from, req_cmd, res_data);
response_string = response.dump();
}
catch (const std::exception &e)
{
json error_response = utils::create_api_response(false, 500, "Processing error: " + std::string(e.what()), "", "", "");
response_string = error_response.dump();
}
return response_string;
}
void KinematicsWebAPI::log(const std::string &message)
{
std::cout << "[" << getCurrentTimestamp() << "] " << message << std::endl;
if (onLog)
{
onLog("[" + getCurrentTimestamp() + "] " + message);
}
}
std::string KinematicsWebAPI::getCurrentTimestamp()
{
return utils::get_current_timestamp();
}
bool KinematicsWebAPI::is_running() const
{
return running_;
}

333
src/Robot.cpp
View File

File diff suppressed because it is too large Load Diff

126
src/api/KinematicsWebAPI.Core.cpp Normal file
View File

@@ -0,0 +1,126 @@
#include "KinematicsWebAPI.h"
#include "URDFStrings.h"
#include <cassert>
#include <ctime>
#include <iostream>
#include <string>
namespace
{
bool isRobotCommand(const std::string &req_cmd)
{
return req_cmd == "Cmd_Kinematics_inverse_pose_str" ||
req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps" ||
req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference" ||
req_cmd == "Cmd_Kinematics_forward_pose_str" ||
req_cmd == "Cmd_Kinematics_forward_all_joints" ||
req_cmd == "Cmd_InitRobot" ||
req_cmd == "Cmd_GetRobot" ||
req_cmd == "Cmd_RemoveRobot" ||
req_cmd == "Cmd_ListRobots" ||
req_cmd == "Cmd_SelectCraftTree" ||
req_cmd == "Cmd_AddOperationTree";
}
} // namespace
KinematicsWebAPI::KinematicsWebAPI()
{
init_func();
}
std::string KinematicsWebAPI::init_func()
{
std::string urdf_string = URDFStrings::abb120_urdf;
RobotManager::initRobot(urdf_string, "9D7EAEF4-1AAB-499E-8783-B6CE016BC6D1");
RobotManager::initRobot(urdf_string, "abb_irb120_3_58");
return utils::create_api_response(true, 200, "init_func", "", "", "").dump();
}
std::string KinematicsWebAPI::func(std::string sanitized_body)
{
log("func called with body: " + sanitized_body.substr(0, 100));
assert(!sanitized_body.empty() && "sanitized_body should not be empty");
try
{
json request_json;
try
{
request_json = json::parse(sanitized_body);
}
catch (const std::exception &)
{
return utils::create_api_response(false, 400, "Invalid JSON format", "", "", "").dump();
}
const std::string msg = request_json.value("msg", "");
const std::string req_code = request_json.value("req_code", "");
const std::string req_from = request_json.value("req_from", "");
const std::string req_cmd = request_json.value("req_cmd", "");
const json req_param = request_json.value("req_param", json::object());
json res_data = dispatchCommand(req_cmd, req_param);
return utils::create_api_response(true, 0, msg, req_code, req_from, req_cmd, res_data).dump();
}
catch (const std::exception &e)
{
return utils::create_api_response(false, 500, "Processing error: " + std::string(e.what()), "", "", "").dump();
}
}
json KinematicsWebAPI::dispatchCommand(const std::string &req_cmd, const json &req_param)
{
if (isRobotCommand(req_cmd))
{
return handleRobotCommand(req_cmd, req_param);
}
if (req_cmd == "Cmd_Spc")
{
return handleSpcCommand(req_cmd, req_param);
}
if (req_cmd == "Cmd_FourBar_CrankSlider")
{
return handleFourBarCommand(req_cmd, req_param);
}
if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles" ||
req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
{
return handleQuadrupedCommand(req_cmd, req_param);
}
return createUnknownCommandResponse(req_cmd, req_param);
}
json KinematicsWebAPI::createUnknownCommandResponse(const std::string &req_cmd, const json &req_param) const
{
return {
{"success", false},
{"error", "Unknown command: " + req_cmd},
{"received_params", req_param},
{"timestamp", std::time(nullptr)}};
}
void KinematicsWebAPI::log(const std::string &message)
{
std::cout << "[" << getCurrentTimestamp() << "] " << message << std::endl;
if (onLog)
{
onLog("[" + getCurrentTimestamp() + "] " + message);
}
}
std::string KinematicsWebAPI::getCurrentTimestamp()
{
return utils::get_current_timestamp();
}
bool KinematicsWebAPI::is_running() const
{
return running_;
}

114
src/api/KinematicsWebAPI.FourBarCommands.cpp Normal file
View File

@@ -0,0 +1,114 @@
#include "KinematicsWebAPI.h"
#include "FourBarMechanism/CrankSliderMechanism.h"
json KinematicsWebAPI::handleFourBarCommand(const std::string &req_cmd, const json &req_param)
{
try
{
log("Handling " + req_cmd + " command");
double L_AB = req_param.value("L_AB", 0.5);
double L_BS = req_param.value("L_BS", 2.0);
double S_OFS = req_param.value("S_OFS", 0.0);
double angleDeg = req_param.value("angleDeg", 0.0);
log("Parameters: L_AB=" + std::to_string(L_AB) +
", L_BS=" + std::to_string(L_BS) +
", S_OFS=" + std::to_string(S_OFS) +
", angleDeg=" + std::to_string(angleDeg));
std::unique_ptr<CrankSliderMechanism> mechanism(createCrankSliderMechanism());
mechanism->setL_AB(L_AB);
mechanism->setL_BS(L_BS);
mechanism->setS_OFS(S_OFS);
ValidationResult validation = mechanism->validateParameters();
if (!validation.isValid())
{
return {
{"success", false},
{"error", "Invalid parameters"},
{"validation_errors", validation.Errors},
{"validation_warnings", validation.Warnings}};
}
MechanismState state = mechanism->calculate(angleDeg);
if (state.hasError())
{
return {
{"success", false},
{"error", state.ErrorMessage}};
}
json result;
result["success"] = true;
json points_json;
for (const auto &point_pair : state.Points)
{
points_json[point_pair.first] = {
{"x", point_pair.second.X},
{"y", point_pair.second.Y}};
}
result["points"] = points_json;
json poses_json;
for (const auto &pose_pair : state.Poses)
{
poses_json[pose_pair.first] = {
{"tx", pose_pair.second.tx},
{"ty", pose_pair.second.ty},
{"tz", pose_pair.second.tz},
{"qx", pose_pair.second.qx},
{"qy", pose_pair.second.qy},
{"qz", pose_pair.second.qz},
{"qw", pose_pair.second.qw}};
}
result["poses"] = poses_json;
json angles_json;
for (const auto &angle_pair : state.Angles)
{
angles_json[angle_pair.first] = angle_pair.second;
}
result["angles"] = angles_json;
result["input_value"] = state.InputValue;
if (state.hasWarning())
{
result["warning"] = state.WarningMessage;
}
json trajectory_json = json::array();
for (const auto &point : mechanism->getTrajectoryPoints())
{
trajectory_json.push_back({
{"x", point.X},
{"y", point.Y}});
}
result["trajectory"] = trajectory_json;
json slider_trajectory_json = json::array();
for (const auto &point : mechanism->getSliderTrajectory())
{
slider_trajectory_json.push_back({
{"x", point.X},
{"y", point.Y}});
}
result["slider_trajectory"] = slider_trajectory_json;
result["parameters"] = {
{"L_AB", L_AB},
{"L_BS", L_BS},
{"S_OFS", S_OFS},
{"angleDeg", angleDeg}};
return result;
}
catch (const std::exception &e)
{
return {
{"success", false},
{"error", "Failed to Cmd_FourBar_CrankSlider: " + std::string(e.what())}};
}
}

21
src/api/KinematicsWebAPI.QuadrupedCommands.cpp Normal file
View File

@@ -0,0 +1,21 @@
#include "KinematicsWebAPI.h"
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
#include <iostream>
json KinematicsWebAPI::handleQuadrupedCommand(const std::string &req_cmd, const json &req_param)
{
if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles")
{
std::cout << "[DEBUG] Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles: 开始计算点位" << std::endl;
return KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(req_param.dump());
}
if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
{
std::cout << "[DEBUG] Cmd_QuadrupedRobot_PerformForwardKinematics: 开始执行正运动学" << std::endl;
return KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(req_param.dump());
}
return createUnknownCommandResponse(req_cmd, req_param);
}

316
src/api/KinematicsWebAPI.RobotCommands.cpp Normal file
View File

@@ -0,0 +1,316 @@
#include "KinematicsWebAPI.h"
json KinematicsWebAPI::handleRobotCommand(const std::string &req_cmd, const json &req_param)
{
json res_data;
if (req_cmd == "Cmd_Kinematics_inverse_pose_str")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
json result = robot->inversePoseStr(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str_2PSteps command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
std::string steps_str = req_param.value("steps_str", "default");
int steps = std::stoi(steps_str);
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
json result = robot->inversePoseStr2PSteps(pose_str, q_init_str, steps);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference")
{
try
{
log("Handling Cmd_Kinematics_inverse_pose_str_NoDifference command");
std::string pose_str = req_param.value("pose_str", "");
std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
json result = robot->inversePoseStrNoDifference(pose_str, q_init_str);
if (result.empty())
{
res_data = {{"error", "Inverse kinematics calculation failed"}};
}
else
{
res_data = {{"joints", result}, {"success", true}};
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_pose_str")
{
try
{
log("Handling Cmd_Kinematics_forward_pose_str command");
std::string joints_str = req_param.value("q_init_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
auto joints = robot->parseJointString(joints_str);
if (joints.size() != 6)
{
res_data = {{"error", "Invalid joints format"}};
}
else
{
double tcp_pose[7];
if (robot->calculateFK_TCP(joints.data(), tcp_pose))
{
res_data = {
{"position", {tcp_pose[0], tcp_pose[1], tcp_pose[2]}},
{"orientation", {tcp_pose[3], tcp_pose[4], tcp_pose[5], tcp_pose[6]}},
{"joints", joints},
{"success", true}};
}
else
{
res_data = {{"error", "Forward kinematics calculation failed"}};
}
}
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_Kinematics_forward_all_joints")
{
try
{
log("Handling Cmd_Kinematics_forward_all_joints command");
std::string joints_str = req_param.value("joints_str", "0,0,0,0,0,0");
std::string robot_uuid = req_param.value("robot_uuid", "default");
auto robot = RobotManager::getRobot(robot_uuid);
if (!robot || !robot->isInitialized())
{
res_data = {{"error", "Robot not found or not initialized"}};
}
else
{
res_data = {
{"OPERATION", robot->handleKinematicsForwardAllJoints(joints_str)},
{"success", true}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_InitRobot")
{
try
{
log("Handling Cmd_InitRobot command");
std::string urdf_base64 = req_param.value("urdf_base64", "");
std::string uuid = req_param.value("robot_uuid", "");
bool force_update = req_param.value("force_update", true);
std::string urdf_content = utils::base64_to_urdf(urdf_base64);
if (!utils::validate_urdf_base64(urdf_base64))
{
res_data = {
{"success", false},
{"message", "Invalid URDF format"},
{"timestamp", utils::get_current_time()}};
}
else
{
auto result = RobotManager::initRobot(urdf_content, uuid, force_update);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to initialize robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_GetRobot")
{
try
{
log("Handling Cmd_GetRobot command");
std::string uuid = req_param.value("uuid", "");
auto robot = RobotManager::getRobot(uuid);
if (!robot)
{
res_data = {{"error", "Robot not found"}};
}
else
{
res_data = {
{"success", true},
{"uuid", uuid},
{"initialized", robot->isInitialized()},
{"joints_count", robot->getNumberOfJoints()},
{"timestamp", utils::get_current_time()}};
}
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to get robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_RemoveRobot")
{
try
{
log("Handling Cmd_RemoveRobot command");
std::string uuid = req_param.value("uuid", "");
auto result = RobotManager::removeRobot(uuid);
res_data = {
{"success", result.first},
{"message", result.second},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to remove robot: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_ListRobots")
{
try
{
log("Handling Cmd_ListRobots command");
bool detail = req_param.value("detail", false);
auto robots = RobotManager::listRobots(detail);
res_data = {
{"success", true},
{"robots", robots},
{"count", robots.size()},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to list robots: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_SelectCraftTree")
{
try
{
log("Handling Cmd_SelectCraftTree command");
std::string tree_id = req_param.value("tree_id", "");
res_data = {
{"success", true},
{"tree_id", tree_id},
{"message", "Craft tree selected successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to select craft tree: " + std::string(e.what())}};
}
}
else if (req_cmd == "Cmd_AddOperationTree")
{
try
{
log("Handling Cmd_AddOperationTree command");
std::string tree_name = req_param.value("name", "");
json operations = req_param.value("operations", json::array());
res_data = {
{"success", true},
{"tree_name", tree_name},
{"operations_count", operations.size()},
{"message", "Operation tree added successfully"},
{"timestamp", utils::get_current_time()}};
}
catch (const std::exception &e)
{
res_data = {{"error", "Failed to add operation tree: " + std::string(e.what())}};
}
}
return res_data;
}

14
src/api/KinematicsWebAPI.SpcCommands.cpp Normal file
View File

@@ -0,0 +1,14 @@
#include "KinematicsWebAPI.h"
json KinematicsWebAPI::handleSpcCommand(const std::string &req_cmd, const json &req_param)
{
try
{
log("Handling " + req_cmd + " command");
return SpcCalculator::Spc(req_param);
}
catch (const std::exception &e)
{
return {{"error", "Failed to Cmd_Spc: " + std::string(e.what())}};
}
}

48
src/utils.cpp
View File

@@ -8,8 +8,6 @@
#include <algorithm>
#include <stdexcept>
#include <stdexcept>
// 实现 splitByDelimiter 方法
std::pair<std::string, std::string> StringUtils::splitByDelimiter(
const std::string &str,
@@ -88,13 +86,13 @@ std::pair<std::string, std::string> StringUtils::splitStrict(
namespace utils
{
// Base64<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD>
// Base64 字符表
const std::string base64_chars =
"ABCDEFGHIJKLMNOPQRSTUVWXYZ"
"abcdefghijklmnopqrstuvwxyz"
"0123456789+/";
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ<EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>base64<EFBFBD>ַ<EFBFBD>
// 判断字符是否为有效的 Base64 字符
static bool is_base64(unsigned char c)
{
return (isalnum(c) || (c == '+') || (c == '/'));
@@ -134,7 +132,7 @@ namespace utils
// res.set_header("Access-Control-Allow-Methods", "GET, POST, PUT, DELETE, OPTIONS, PATCH");
// res.set_header("Access-Control-Allow-Headers", "Content-Type, Authorization, X-Requested-With, X-API-Key, Accept, Origin");
// res.set_header("Access-Control-Expose-Headers", "Content-Length, Content-Type, X-Request-Id");
// res.set_header("Access-Control-Max-Age", "86400"); // 24Сʱ
// res.set_header("Access-Control-Max-Age", "86400"); // 24 小时
// res.set_header("Vary", "Origin");
// }
@@ -171,11 +169,11 @@ namespace utils
unsigned char c = str[i];
if (c <= 0x7F)
{
continue; // ASCII<EFBFBD>ַ<EFBFBD>
continue; // ASCII 字符
}
else if ((c & 0xE0) == 0xC0)
{
// 2<EFBFBD>ֽ<EFBFBD>UTF-8
// 2 字节 UTF-8
if (i + 1 >= str.size() || (str[i + 1] & 0xC0) != 0x80)
{
return false;
@@ -184,7 +182,7 @@ namespace utils
}
else if ((c & 0xF0) == 0xE0)
{
// 3<EFBFBD>ֽ<EFBFBD>UTF-8
// 3 字节 UTF-8
if (i + 2 >= str.size() || (str[i + 1] & 0xC0) != 0x80 || (str[i + 2] & 0xC0) != 0x80)
{
return false;
@@ -193,7 +191,7 @@ namespace utils
}
else if ((c & 0xF8) == 0xF0)
{
// 4<EFBFBD>ֽ<EFBFBD>UTF-8
// 4 字节 UTF-8
if (i + 3 >= str.size() || (str[i + 1] & 0xC0) != 0x80 ||
(str[i + 2] & 0xC0) != 0x80 || (str[i + 3] & 0xC0) != 0x80)
{
@@ -203,7 +201,7 @@ namespace utils
}
else
{
return false; // <EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>UTF-8<EFBFBD>ֽ<EFBFBD>
return false; // 非法 UTF-8 字节
}
}
return true;
@@ -219,11 +217,11 @@ namespace utils
unsigned char c = str[i];
if (c <= 0x7F)
{
result += c; // ASCII<EFBFBD>ַ<EFBFBD>
result += c; // ASCII 字符
}
else if ((c & 0xE0) == 0xC0)
{
// 2<EFBFBD>ֽ<EFBFBD>UTF-8
// 2 字节 UTF-8
if (i + 1 < str.size() && (str[i + 1] & 0xC0) == 0x80)
{
result += c;
@@ -233,7 +231,7 @@ namespace utils
}
else if ((c & 0xF0) == 0xE0)
{
// 3<EFBFBD>ֽ<EFBFBD>UTF-8
// 3 字节 UTF-8
if (i + 2 < str.size() && (str[i + 1] & 0xC0) == 0x80 && (str[i + 2] & 0xC0) == 0x80)
{
result += c;
@@ -244,7 +242,7 @@ namespace utils
}
else if ((c & 0xF8) == 0xF0)
{
// 4<EFBFBD>ֽ<EFBFBD>UTF-8
// 4 字节 UTF-8
if (i + 3 < str.size() && (str[i + 1] & 0xC0) == 0x80 &&
(str[i + 2] & 0xC0) == 0x80 && (str[i + 3] & 0xC0) == 0x80)
{
@@ -255,7 +253,7 @@ namespace utils
i += 3;
}
}
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>UTF-8<EFBFBD>ֽ<EFBFBD>
// 跳过非法 UTF-8 字节
}
return result;
}
@@ -285,7 +283,7 @@ namespace utils
return response;
}
// Base64<EFBFBD><EFBFBD><EFBFBD>뺯<EFBFBD><EFBFBD>
// Base64 解码函数
std::string base64_decode(const std::string &encoded_string)
{
int in_len = encoded_string.size();
@@ -295,7 +293,7 @@ namespace utils
unsigned char char_array_4[4], char_array_3[3];
std::string ret;
// <EFBFBD>Ƴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܵĻ<EFBFBD><EFBFBD>з<EFBFBD><EFBFBD>Ϳո<EFBFBD>
// 去掉输入中的换行和空格
std::string clean_encoded;
for (char c : encoded_string)
{
@@ -346,7 +344,7 @@ namespace utils
size_t pos = base64_chars.find(char_array_4[j]);
if (pos == std::string::npos && j >= i)
{
// <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD>λ<EFBFBD><EFBFBD>Ϊ0
// 补齐的占位字符按 0 处理
char_array_4[j] = 0;
}
else if (pos != std::string::npos)
@@ -372,14 +370,14 @@ namespace utils
return ret;
}
// <EFBFBD><EFBFBD>base64<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>URDFת<EFBFBD><EFBFBD>Ϊ<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD>
// 将 Base64 编码的 URDF 转成字符串
std::string base64_to_urdf(const std::string &base64_urdf)
{
try
{
std::string urdf_content = base64_decode(base64_urdf);
// <EFBFBD><EFBFBD>֤<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>URDF/XML
// 校验解码结果是否像合法的 URDF/XML
if (urdf_content.find("<?xml") != std::string::npos ||
urdf_content.find("<robot") != std::string::npos)
{
@@ -396,7 +394,7 @@ namespace utils
}
}
// <EFBFBD><EFBFBD>base64<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>URDF<EFBFBD><EFBFBD><EFBFBD>浽<EFBFBD>ļ<EFBFBD>
// 将 Base64 编码的 URDF 保存到文件
bool save_base64_urdf_to_file(const std::string &base64_urdf, const std::string &filename)
{
try
@@ -411,7 +409,7 @@ namespace utils
}
}
// <EFBFBD><EFBFBD>URDF<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>浽<EFBFBD>ļ<EFBFBD>
// 将 URDF 字符串保存到文件
bool save_urdf_string_to_file(const std::string &urdf_content, const std::string &filename)
{
try
@@ -437,14 +435,14 @@ namespace utils
}
}
// <EFBFBD><EFBFBD>֤base64<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD>URDF
// 校验 Base64 字符串是否可解析为合法 URDF
bool validate_urdf_base64(const std::string &base64_urdf)
{
try
{
std::string urdf_content = base64_to_urdf(base64_urdf);
// <EFBFBD>򵥵<EFBFBD>URDF<EFBFBD><EFBFBD>֤
// 基础 URDF 结构校验
bool has_xml_decl = urdf_content.find("<?xml") != std::string::npos;
bool has_robot_tag = urdf_content.find("<robot") != std::string::npos;
bool has_link_tag = urdf_content.find("<link") != std::string::npos;
@@ -458,4 +456,4 @@ namespace utils
}
}
} // namespace utils
} // namespace utils