按功能拆分接口并清理核心乱码注释
This commit is contained in:
zhangshun
316
src/api/KinematicsWebAPI.RobotCommands.cpp
Normal file
316
src/api/KinematicsWebAPI.RobotCommands.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user