847 lines
33 KiB
C++
847 lines
33 KiB
C++
#include "QuadrupedRobotSimulation/RobotConfig.hpp"
|
||
// 统一的更新函数：手动从JSON更新现有对象
|
||
RequestParameters &RequestParameters::updateFromJson(const nlohmann::json &j, RequestParameters &req_param)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return req_param; // 修正：返回传入的引用
|
||
|
||
try
|
||
{
|
||
// 更新 GaitInfo
|
||
if (j.contains("GaitInfo"))
|
||
{
|
||
const auto &gaitJson = j.at("GaitInfo");
|
||
if (gaitJson.is_object())
|
||
{
|
||
if (gaitJson.contains("GaitType"))
|
||
req_param.GaitInfo.GaitType = gaitJson.at("GaitType").get<std::string>();
|
||
if (gaitJson.contains("Period"))
|
||
req_param.GaitInfo.Period = gaitJson.at("Period").get<double>();
|
||
if (gaitJson.contains("Frequency"))
|
||
req_param.GaitInfo.Frequency = gaitJson.at("Frequency").get<double>();
|
||
if (gaitJson.contains("StepTime"))
|
||
req_param.GaitInfo.StepTime = gaitJson.at("StepTime").get<double>();
|
||
if (gaitJson.contains("SupportTime"))
|
||
req_param.GaitInfo.SupportTime = gaitJson.at("SupportTime").get<double>();
|
||
if (gaitJson.contains("SwingTime"))
|
||
req_param.GaitInfo.SwingTime = gaitJson.at("SwingTime").get<double>();
|
||
if (gaitJson.contains("SupportRatio"))
|
||
req_param.GaitInfo.SupportRatio = gaitJson.at("SupportRatio").get<double>();
|
||
if (gaitJson.contains("SwingRatio"))
|
||
req_param.GaitInfo.SwingRatio = gaitJson.at("SwingRatio").get<double>();
|
||
if (gaitJson.contains("TotalFrames"))
|
||
req_param.GaitInfo.TotalFrames = gaitJson.at("TotalFrames").get<int>();
|
||
if (gaitJson.contains("TotalTime"))
|
||
req_param.GaitInfo.TotalTime = gaitJson.at("TotalTime").get<double>();
|
||
}
|
||
}
|
||
|
||
// 更新 SystemParameters
|
||
if (j.contains("SystemParameters"))
|
||
{
|
||
const auto &sysJson = j.at("SystemParameters");
|
||
if (sysJson.is_object())
|
||
{
|
||
if (sysJson.contains("A_x"))
|
||
req_param.SystemParameters.A_x = sysJson.at("A_x").get<double>();
|
||
if (sysJson.contains("A_y"))
|
||
req_param.SystemParameters.A_y = sysJson.at("A_y").get<double>();
|
||
if (sysJson.contains("L10"))
|
||
req_param.SystemParameters.L10 = sysJson.at("L10").get<double>();
|
||
if (sysJson.contains("L20"))
|
||
req_param.SystemParameters.L20 = sysJson.at("L20").get<double>();
|
||
if (sysJson.contains("L30"))
|
||
req_param.SystemParameters.L30 = sysJson.at("L30").get<double>();
|
||
if (sysJson.contains("StepLength"))
|
||
req_param.SystemParameters.StepLength = sysJson.at("StepLength").get<double>();
|
||
if (sysJson.contains("StepHeight"))
|
||
req_param.SystemParameters.StepHeight = sysJson.at("StepHeight").get<double>();
|
||
if (sysJson.contains("X"))
|
||
req_param.SystemParameters.X = sysJson.at("X").get<double>();
|
||
if (sysJson.contains("DeltaX"))
|
||
req_param.SystemParameters.DeltaX = sysJson.at("DeltaX").get<double>();
|
||
if (sysJson.contains("L21"))
|
||
req_param.SystemParameters.L21 = sysJson.at("L21").get<double>();
|
||
if (sysJson.contains("L22"))
|
||
req_param.SystemParameters.L22 = sysJson.at("L22").get<double>();
|
||
if (sysJson.contains("L23"))
|
||
req_param.SystemParameters.L23 = sysJson.at("L23").get<double>();
|
||
if (sysJson.contains("Beta1"))
|
||
req_param.SystemParameters.Beta1 = sysJson.at("Beta1").get<double>();
|
||
if (sysJson.contains("BB2_BC_Angle"))
|
||
req_param.SystemParameters.BB2_BC_Angle = sysJson.at("BB2_BC_Angle").get<double>();
|
||
if (sysJson.contains("C1_B_Length"))
|
||
req_param.SystemParameters.C1_B_Length = sysJson.at("C1_B_Length").get<double>();
|
||
if (sysJson.contains("CC1"))
|
||
req_param.SystemParameters.CC1 = sysJson.at("CC1").get<double>();
|
||
if (sysJson.contains("L31"))
|
||
req_param.SystemParameters.L31 = sysJson.at("L31").get<double>();
|
||
if (sysJson.contains("C2C3_Length"))
|
||
req_param.SystemParameters.C2C3_Length = sysJson.at("C2C3_Length").get<double>();
|
||
if (sysJson.contains("Lead"))
|
||
req_param.SystemParameters.Lead = sysJson.at("Lead").get<double>();
|
||
if (sysJson.contains("D1_L_Offset"))
|
||
req_param.SystemParameters.D1_L_Offset = sysJson.at("D1_L_Offset").get<double>();
|
||
if (sysJson.contains("D2_L_Offset"))
|
||
req_param.SystemParameters.D2_L_Offset = sysJson.at("D2_L_Offset").get<double>();
|
||
if (sysJson.contains("C4_D2_Offset"))
|
||
req_param.SystemParameters.C4_D2_Offset = sysJson.at("C4_D2_Offset").get<double>();
|
||
if (sysJson.contains("ThighMotorReduction"))
|
||
req_param.SystemParameters.ThighMotorReduction = sysJson.at("ThighMotorReduction").get<double>();
|
||
if (sysJson.contains("ShankMotorReduction"))
|
||
req_param.SystemParameters.ShankMotorReduction = sysJson.at("ShankMotorReduction").get<double>();
|
||
if (sysJson.contains("AnkleMotorReduction"))
|
||
req_param.SystemParameters.AnkleMotorReduction = sysJson.at("AnkleMotorReduction").get<double>();
|
||
}
|
||
}
|
||
|
||
// 更新 ModelID
|
||
if (j.contains("ModelID"))
|
||
{
|
||
const auto &modelJson = j.at("ModelID");
|
||
if (modelJson.is_object())
|
||
{
|
||
// 更新 LF
|
||
if (modelJson.contains("LF"))
|
||
{
|
||
const auto &lfJson = modelJson.at("LF");
|
||
if (lfJson.is_array())
|
||
{
|
||
req_param.ModelID.LF.components.clear();
|
||
for (const auto &item : lfJson)
|
||
{
|
||
if (item.is_object())
|
||
{
|
||
ComponentID comp;
|
||
auto it = item.begin();
|
||
if (it != item.end())
|
||
{
|
||
comp.name = it.key();
|
||
comp.uuid = it.value().get<std::string>();
|
||
req_param.ModelID.LF.components.push_back(comp);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 更新 LH
|
||
if (modelJson.contains("LH"))
|
||
{
|
||
const auto &lhJson = modelJson.at("LH");
|
||
if (lhJson.is_array())
|
||
{
|
||
req_param.ModelID.LH.components.clear();
|
||
for (const auto &item : lhJson)
|
||
{
|
||
if (item.is_object())
|
||
{
|
||
ComponentID comp;
|
||
auto it = item.begin();
|
||
if (it != item.end())
|
||
{
|
||
comp.name = it.key();
|
||
comp.uuid = it.value().get<std::string>();
|
||
req_param.ModelID.LH.components.push_back(comp);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 更新 RF
|
||
if (modelJson.contains("RF"))
|
||
{
|
||
const auto &rfJson = modelJson.at("RF");
|
||
if (rfJson.is_array())
|
||
{
|
||
req_param.ModelID.RF.components.clear();
|
||
for (const auto &item : rfJson)
|
||
{
|
||
if (item.is_object())
|
||
{
|
||
ComponentID comp;
|
||
auto it = item.begin();
|
||
if (it != item.end())
|
||
{
|
||
comp.name = it.key();
|
||
comp.uuid = it.value().get<std::string>();
|
||
req_param.ModelID.RF.components.push_back(comp);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 更新 RH
|
||
if (modelJson.contains("RH"))
|
||
{
|
||
const auto &rhJson = modelJson.at("RH");
|
||
if (rhJson.is_array())
|
||
{
|
||
req_param.ModelID.RH.components.clear();
|
||
for (const auto &item : rhJson)
|
||
{
|
||
if (item.is_object())
|
||
{
|
||
ComponentID comp;
|
||
auto it = item.begin();
|
||
if (it != item.end())
|
||
{
|
||
comp.name = it.key();
|
||
comp.uuid = it.value().get<std::string>();
|
||
req_param.ModelID.RH.components.push_back(comp);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 更新 Param (LegParameters)
|
||
if (j.contains("Param"))
|
||
{
|
||
const auto ¶mJson = j.at("Param");
|
||
if (paramJson.is_object())
|
||
{
|
||
// 更新 LF
|
||
if (paramJson.contains("LF"))
|
||
{
|
||
const auto &lfJson = paramJson.at("LF");
|
||
if (lfJson.is_object())
|
||
{
|
||
if (lfJson.contains("thigh_angle_deg"))
|
||
req_param.Param.LF.thigh_angle_deg = lfJson.at("thigh_angle_deg").get<double>();
|
||
if (lfJson.contains("shank_angle_deg"))
|
||
req_param.Param.LF.shank_angle_deg = lfJson.at("shank_angle_deg").get<double>();
|
||
if (lfJson.contains("ankle_angle_deg"))
|
||
req_param.Param.LF.ankle_angle_deg = lfJson.at("ankle_angle_deg").get<double>();
|
||
}
|
||
}
|
||
|
||
// 更新 LH
|
||
if (paramJson.contains("LH"))
|
||
{
|
||
const auto &lhJson = paramJson.at("LH");
|
||
if (lhJson.is_object())
|
||
{
|
||
if (lhJson.contains("thigh_angle_deg"))
|
||
req_param.Param.LH.thigh_angle_deg = lhJson.at("thigh_angle_deg").get<double>();
|
||
if (lhJson.contains("shank_angle_deg"))
|
||
req_param.Param.LH.shank_angle_deg = lhJson.at("shank_angle_deg").get<double>();
|
||
if (lhJson.contains("ankle_angle_deg"))
|
||
req_param.Param.LH.ankle_angle_deg = lhJson.at("ankle_angle_deg").get<double>();
|
||
}
|
||
}
|
||
|
||
// 更新 RF
|
||
if (paramJson.contains("RF"))
|
||
{
|
||
const auto &rfJson = paramJson.at("RF");
|
||
if (rfJson.is_object())
|
||
{
|
||
if (rfJson.contains("thigh_angle_deg"))
|
||
req_param.Param.RF.thigh_angle_deg = rfJson.at("thigh_angle_deg").get<double>();
|
||
if (rfJson.contains("shank_angle_deg"))
|
||
req_param.Param.RF.shank_angle_deg = rfJson.at("shank_angle_deg").get<double>();
|
||
if (rfJson.contains("ankle_angle_deg"))
|
||
req_param.Param.RF.ankle_angle_deg = rfJson.at("ankle_angle_deg").get<double>();
|
||
}
|
||
}
|
||
|
||
// 更新 RH
|
||
if (paramJson.contains("RH"))
|
||
{
|
||
const auto &rhJson = paramJson.at("RH");
|
||
if (rhJson.is_object())
|
||
{
|
||
if (rhJson.contains("thigh_angle_deg"))
|
||
req_param.Param.RH.thigh_angle_deg = rhJson.at("thigh_angle_deg").get<double>();
|
||
if (rhJson.contains("shank_angle_deg"))
|
||
req_param.Param.RH.shank_angle_deg = rhJson.at("shank_angle_deg").get<double>();
|
||
if (rhJson.contains("ankle_angle_deg"))
|
||
req_param.Param.RH.ankle_angle_deg = rhJson.at("ankle_angle_deg").get<double>();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 更新 RobotBody
|
||
if (j.contains("RobotBody"))
|
||
{
|
||
const auto &bodyJson = j.at("RobotBody");
|
||
if (bodyJson.is_object())
|
||
{
|
||
if (bodyJson.contains("BodyCode"))
|
||
req_param.RobotBody.BodyCode = bodyJson.at("BodyCode").get<std::string>();
|
||
if (bodyJson.contains("x"))
|
||
req_param.RobotBody.x = bodyJson.at("x").get<double>();
|
||
if (bodyJson.contains("y"))
|
||
req_param.RobotBody.y = bodyJson.at("y").get<double>();
|
||
if (bodyJson.contains("z"))
|
||
req_param.RobotBody.z = bodyJson.at("z").get<double>();
|
||
if (bodyJson.contains("qx"))
|
||
req_param.RobotBody.qx = bodyJson.at("qx").get<double>();
|
||
if (bodyJson.contains("qy"))
|
||
req_param.RobotBody.qy = bodyJson.at("qy").get<double>();
|
||
if (bodyJson.contains("qz"))
|
||
req_param.RobotBody.qz = bodyJson.at("qz").get<double>();
|
||
if (bodyJson.contains("qw"))
|
||
req_param.RobotBody.qw = bodyJson.at("qw").get<double>();
|
||
}
|
||
}
|
||
|
||
// 更新 t_percentage
|
||
if (j.contains("t_percentage"))
|
||
{
|
||
req_param.t_percentage = j.at("t_percentage").get<double>();
|
||
}
|
||
if (j.contains("RobotID"))
|
||
{
|
||
req_param.RobotID = j.at("RobotID").get<std::string>();
|
||
}
|
||
|
||
return req_param; // 返回引用
|
||
}
|
||
catch (const nlohmann::json::exception &e)
|
||
{
|
||
std::cerr << "[ERROR] 更新RequestParameters失败: " << e.what() << std::endl;
|
||
std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
|
||
throw;
|
||
}
|
||
}
|
||
// GaitInfo 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const GaitInfo &g)
|
||
{
|
||
j = nlohmann::json{
|
||
{"GaitType", g.GaitType},
|
||
{"Period", g.Period},
|
||
{"Frequency", g.Frequency},
|
||
{"StepTime", g.StepTime},
|
||
{"SupportTime", g.SupportTime},
|
||
{"SwingTime", g.SwingTime},
|
||
{"SupportRatio", g.SupportRatio},
|
||
{"SwingRatio", g.SwingRatio},
|
||
{"TotalFrames", g.TotalFrames},
|
||
{"TotalTime", g.TotalTime}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, GaitInfo &g)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
if (j.contains("GaitType"))
|
||
j.at("GaitType").get_to(g.GaitType);
|
||
if (j.contains("Period"))
|
||
j.at("Period").get_to(g.Period);
|
||
if (j.contains("Frequency"))
|
||
j.at("Frequency").get_to(g.Frequency);
|
||
if (j.contains("StepTime"))
|
||
j.at("StepTime").get_to(g.StepTime);
|
||
if (j.contains("SupportTime"))
|
||
j.at("SupportTime").get_to(g.SupportTime);
|
||
if (j.contains("SwingTime"))
|
||
j.at("SwingTime").get_to(g.SwingTime);
|
||
if (j.contains("SupportRatio"))
|
||
j.at("SupportRatio").get_to(g.SupportRatio);
|
||
if (j.contains("SwingRatio"))
|
||
j.at("SwingRatio").get_to(g.SwingRatio);
|
||
if (j.contains("TotalFrames"))
|
||
j.at("TotalFrames").get_to(g.TotalFrames);
|
||
if (j.contains("TotalTime"))
|
||
j.at("TotalTime").get_to(g.TotalTime);
|
||
}
|
||
|
||
// SystemParameters 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const SystemParameters &s)
|
||
{
|
||
j = nlohmann::json{
|
||
{"A_x", s.A_x},
|
||
{"A_y", s.A_y},
|
||
{"L10", s.L10},
|
||
{"L20", s.L20},
|
||
{"L30", s.L30},
|
||
{"StepLength", s.StepLength},
|
||
{"StepHeight", s.StepHeight},
|
||
{"X", s.X},
|
||
{"DeltaX", s.DeltaX},
|
||
{"L21", s.L21},
|
||
{"L22", s.L22},
|
||
{"L23", s.L23},
|
||
{"Beta1", s.Beta1},
|
||
{"BB2_BC_Angle", s.BB2_BC_Angle},
|
||
{"C1_B_Length", s.C1_B_Length},
|
||
{"CC1", s.CC1},
|
||
{"L31", s.L31},
|
||
{"C2C3_Length", s.C2C3_Length},
|
||
{"Lead", s.Lead},
|
||
{"D1_L_Offset", s.D1_L_Offset},
|
||
{"D2_L_Offset", s.D2_L_Offset},
|
||
{"C4_D2_Offset", s.C4_D2_Offset},
|
||
{"ThighMotorReduction", s.ThighMotorReduction},
|
||
{"ShankMotorReduction", s.ShankMotorReduction},
|
||
{"AnkleMotorReduction", s.AnkleMotorReduction}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, SystemParameters &s)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
if (j.contains("A_x"))
|
||
j.at("A_x").get_to(s.A_x);
|
||
if (j.contains("A_y"))
|
||
j.at("A_y").get_to(s.A_y);
|
||
if (j.contains("L10"))
|
||
j.at("L10").get_to(s.L10);
|
||
if (j.contains("L20"))
|
||
j.at("L20").get_to(s.L20);
|
||
if (j.contains("L30"))
|
||
j.at("L30").get_to(s.L30);
|
||
if (j.contains("StepLength"))
|
||
j.at("StepLength").get_to(s.StepLength);
|
||
if (j.contains("StepHeight"))
|
||
j.at("StepHeight").get_to(s.StepHeight);
|
||
if (j.contains("X"))
|
||
j.at("X").get_to(s.X);
|
||
if (j.contains("DeltaX"))
|
||
j.at("DeltaX").get_to(s.DeltaX);
|
||
if (j.contains("L21"))
|
||
j.at("L21").get_to(s.L21);
|
||
if (j.contains("L22"))
|
||
j.at("L22").get_to(s.L22);
|
||
if (j.contains("L23"))
|
||
j.at("L23").get_to(s.L23);
|
||
if (j.contains("Beta1"))
|
||
j.at("Beta1").get_to(s.Beta1);
|
||
if (j.contains("BB2_BC_Angle"))
|
||
j.at("BB2_BC_Angle").get_to(s.BB2_BC_Angle);
|
||
if (j.contains("C1_B_Length"))
|
||
j.at("C1_B_Length").get_to(s.C1_B_Length);
|
||
if (j.contains("CC1"))
|
||
j.at("CC1").get_to(s.CC1);
|
||
if (j.contains("L31"))
|
||
j.at("L31").get_to(s.L31);
|
||
if (j.contains("C2C3_Length"))
|
||
j.at("C2C3_Length").get_to(s.C2C3_Length);
|
||
if (j.contains("Lead"))
|
||
j.at("Lead").get_to(s.Lead);
|
||
if (j.contains("D1_L_Offset"))
|
||
j.at("D1_L_Offset").get_to(s.D1_L_Offset);
|
||
if (j.contains("D2_L_Offset"))
|
||
j.at("D2_L_Offset").get_to(s.D2_L_Offset);
|
||
if (j.contains("C4_D2_Offset"))
|
||
j.at("C4_D2_Offset").get_to(s.C4_D2_Offset);
|
||
if (j.contains("ThighMotorReduction"))
|
||
j.at("ThighMotorReduction").get_to(s.ThighMotorReduction);
|
||
if (j.contains("ShankMotorReduction"))
|
||
j.at("ShankMotorReduction").get_to(s.ShankMotorReduction);
|
||
if (j.contains("AnkleMotorReduction"))
|
||
j.at("AnkleMotorReduction").get_to(s.AnkleMotorReduction);
|
||
}
|
||
|
||
// ComponentID 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const ComponentID &c)
|
||
{
|
||
j = nlohmann::json{{c.name, c.uuid}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, ComponentID &c)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
auto it = j.begin();
|
||
if (it != j.end())
|
||
{
|
||
c.name = it.key();
|
||
c.uuid = it.value();
|
||
}
|
||
}
|
||
|
||
// LegModelIDs 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const LegModelIDs &l)
|
||
{
|
||
|
||
j = nlohmann::json::array();
|
||
for (const auto &component : l.components)
|
||
{
|
||
j.push_back(component);
|
||
}
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, LegModelIDs &l)
|
||
{
|
||
l.components.clear();
|
||
if (j.is_null() || !j.is_array())
|
||
return;
|
||
|
||
for (const auto &item : j)
|
||
{
|
||
ComponentID comp;
|
||
from_json(item, comp);
|
||
l.components.push_back(comp);
|
||
}
|
||
}
|
||
|
||
// ModelID 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const ModelID &m)
|
||
{
|
||
j = nlohmann::json{
|
||
{"LF", m.LF},
|
||
{"LH", m.LH},
|
||
{"RF", m.RF},
|
||
{"RH", m.RH}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, ModelID &m)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
if (j.contains("LF"))
|
||
j.at("LF").get_to(m.LF);
|
||
if (j.contains("LH"))
|
||
j.at("LH").get_to(m.LH);
|
||
if (j.contains("RF"))
|
||
j.at("RF").get_to(m.RF);
|
||
if (j.contains("RH"))
|
||
j.at("RH").get_to(m.RH);
|
||
}
|
||
|
||
// LegParam 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const LegParam &l)
|
||
{
|
||
j = nlohmann::json{
|
||
{"thigh_angle_deg", l.thigh_angle_deg},
|
||
{"shank_angle_deg", l.shank_angle_deg},
|
||
{"ankle_angle_deg", l.ankle_angle_deg}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, LegParam &l)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
if (j.contains("thigh_angle_deg"))
|
||
j.at("thigh_angle_deg").get_to(l.thigh_angle_deg);
|
||
if (j.contains("shank_angle_deg"))
|
||
j.at("shank_angle_deg").get_to(l.shank_angle_deg);
|
||
if (j.contains("ankle_angle_deg"))
|
||
j.at("ankle_angle_deg").get_to(l.ankle_angle_deg);
|
||
}
|
||
|
||
// LegParameters 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const LegParameters &l)
|
||
{
|
||
j = nlohmann::json{
|
||
{"LF", l.LF},
|
||
{"LH", l.LH},
|
||
{"RF", l.RF},
|
||
{"RH", l.RH}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, LegParameters &l)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
if (j.contains("LF"))
|
||
j.at("LF").get_to(l.LF);
|
||
if (j.contains("LH"))
|
||
j.at("LH").get_to(l.LH);
|
||
if (j.contains("RF"))
|
||
j.at("RF").get_to(l.RF);
|
||
if (j.contains("RH"))
|
||
j.at("RH").get_to(l.RH);
|
||
}
|
||
|
||
// RobotBody 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const RobotBody &r)
|
||
{
|
||
j = nlohmann::json{
|
||
{"BodyCode", r.BodyCode},
|
||
{"x", r.x},
|
||
{"y", r.y},
|
||
{"z", r.z},
|
||
{"qx", r.qx},
|
||
{"qy", r.qy},
|
||
{"qz", r.qz},
|
||
{"qw", r.qw}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, RobotBody &r)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
if (j.contains("BodyCode"))
|
||
j.at("BodyCode").get_to(r.BodyCode);
|
||
if (j.contains("x"))
|
||
j.at("x").get_to(r.x);
|
||
if (j.contains("y"))
|
||
j.at("y").get_to(r.y);
|
||
if (j.contains("z"))
|
||
j.at("z").get_to(r.z);
|
||
if (j.contains("qx"))
|
||
j.at("qx").get_to(r.qx);
|
||
if (j.contains("qy"))
|
||
j.at("qy").get_to(r.qy);
|
||
if (j.contains("qz"))
|
||
j.at("qz").get_to(r.qz);
|
||
if (j.contains("qw"))
|
||
j.at("qw").get_to(r.qw);
|
||
}
|
||
|
||
// RequestParameters 序列化/反序列化实现
|
||
void to_json(nlohmann::json &j, const RequestParameters &c)
|
||
{
|
||
j = nlohmann::json{
|
||
{"GaitInfo", c.GaitInfo},
|
||
{"SystemParameters", c.SystemParameters},
|
||
{"ModelID", c.ModelID},
|
||
{"Param", c.Param},
|
||
{"RobotBody", c.RobotBody},
|
||
{"RobotID", c.RobotID},
|
||
{"t_percentage", c.t_percentage}};
|
||
}
|
||
|
||
void from_json(const nlohmann::json &j, RequestParameters &c)
|
||
{
|
||
if (j.is_null() || !j.is_object())
|
||
return;
|
||
|
||
try
|
||
{
|
||
if (j.contains("GaitInfo"))
|
||
j.at("GaitInfo").get_to(c.GaitInfo);
|
||
if (j.contains("SystemParameters"))
|
||
j.at("SystemParameters").get_to(c.SystemParameters);
|
||
if (j.contains("ModelID"))
|
||
j.at("ModelID").get_to(c.ModelID);
|
||
if (j.contains("Param"))
|
||
j.at("Param").get_to(c.Param);
|
||
if (j.contains("RobotBody"))
|
||
j.at("RobotBody").get_to(c.RobotBody);
|
||
if (j.contains("t_percentage"))
|
||
j.at("t_percentage").get_to(c.t_percentage);
|
||
if (j.contains("RobotID"))
|
||
j.at("RobotID").get_to(c.RobotID);
|
||
}
|
||
catch (const nlohmann::json::exception &e)
|
||
{
|
||
std::cerr << "[ERROR] 解析RequestParameters失败: " << e.what() << std::endl;
|
||
std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
|
||
throw;
|
||
}
|
||
}
|
||
void RequestParameters::debugPrint() const
|
||
{
|
||
std::cout << "========================================" << std::endl;
|
||
std::cout << "RequestParameters DEBUG PRINT" << std::endl;
|
||
std::cout << "========================================" << std::endl;
|
||
|
||
// GaitInfo
|
||
std::cout << "=== GaitInfo ===" << std::endl;
|
||
std::cout << " GaitType: " << GaitInfo.GaitType << std::endl;
|
||
std::cout << " Period: " << GaitInfo.Period << std::endl;
|
||
std::cout << " Frequency: " << GaitInfo.Frequency << std::endl;
|
||
std::cout << " StepTime: " << GaitInfo.StepTime << std::endl;
|
||
std::cout << " SupportTime: " << GaitInfo.SupportTime << std::endl;
|
||
std::cout << " SwingTime: " << GaitInfo.SwingTime << std::endl;
|
||
std::cout << " SupportRatio: " << GaitInfo.SupportRatio << std::endl;
|
||
std::cout << " SwingRatio: " << GaitInfo.SwingRatio << std::endl;
|
||
std::cout << " TotalFrames: " << GaitInfo.TotalFrames << std::endl;
|
||
std::cout << " TotalTime: " << GaitInfo.TotalTime << std::endl;
|
||
|
||
// SystemParameters (关键字段)
|
||
std::cout << "\n=== SystemParameters (key fields) ===" << std::endl;
|
||
std::cout << " A_x: " << SystemParameters.A_x << std::endl;
|
||
std::cout << " A_y: " << SystemParameters.A_y << std::endl;
|
||
std::cout << " L10: " << SystemParameters.L10 << std::endl;
|
||
std::cout << " L20: " << SystemParameters.L20 << std::endl;
|
||
std::cout << " L30: " << SystemParameters.L30 << std::endl;
|
||
std::cout << " StepLength: " << SystemParameters.StepLength << std::endl;
|
||
std::cout << " StepHeight: " << SystemParameters.StepHeight << std::endl;
|
||
std::cout << " X: " << SystemParameters.X << std::endl;
|
||
std::cout << " DeltaX: " << SystemParameters.DeltaX << std::endl;
|
||
std::cout << " L21: " << SystemParameters.L21 << std::endl;
|
||
std::cout << " L22: " << SystemParameters.L22 << std::endl;
|
||
std::cout << " L23: " << SystemParameters.L23 << std::endl;
|
||
std::cout << " Beta1: " << SystemParameters.Beta1 << std::endl;
|
||
std::cout << " BB2_BC_Angle: " << SystemParameters.BB2_BC_Angle << std::endl;
|
||
std::cout << " C1_B_Length: " << SystemParameters.C1_B_Length << std::endl;
|
||
std::cout << " CC1: " << SystemParameters.CC1 << std::endl;
|
||
std::cout << " L31: " << SystemParameters.L31 << std::endl;
|
||
std::cout << " C2C3_Length: " << SystemParameters.C2C3_Length << std::endl;
|
||
std::cout << " Lead: " << SystemParameters.Lead << std::endl;
|
||
std::cout << " D1_L_Offset: " << SystemParameters.D1_L_Offset << std::endl;
|
||
std::cout << " D2_L_Offset: " << SystemParameters.D2_L_Offset << std::endl;
|
||
std::cout << " C4_D2_Offset: " << SystemParameters.C4_D2_Offset << std::endl;
|
||
std::cout << " ThighMotorReduction: " << SystemParameters.ThighMotorReduction << std::endl;
|
||
std::cout << " ShankMotorReduction: " << SystemParameters.ShankMotorReduction << std::endl;
|
||
std::cout << " AnkleMotorReduction: " << SystemParameters.AnkleMotorReduction << std::endl;
|
||
|
||
// ModelID详细内容
|
||
std::cout << "\n=== ModelID Details ===" << std::endl;
|
||
|
||
// LF
|
||
std::cout << " LF [" << ModelID.LF.components.size() << " components]:" << std::endl;
|
||
for (size_t i = 0; i < ModelID.LF.components.size(); ++i)
|
||
{
|
||
std::cout << " [" << i << "] " << ModelID.LF.components[i].name
|
||
<< ": " << ModelID.LF.components[i].uuid << std::endl;
|
||
}
|
||
|
||
// LH
|
||
std::cout << " LH [" << ModelID.LH.components.size() << " components]:" << std::endl;
|
||
for (size_t i = 0; i < ModelID.LH.components.size(); ++i)
|
||
{
|
||
std::cout << " [" << i << "] " << ModelID.LH.components[i].name
|
||
<< ": " << ModelID.LH.components[i].uuid << std::endl;
|
||
}
|
||
|
||
// RF
|
||
std::cout << " RF [" << ModelID.RF.components.size() << " components]:" << std::endl;
|
||
for (size_t i = 0; i < ModelID.RF.components.size(); ++i)
|
||
{
|
||
std::cout << " [" << i << "] " << ModelID.RF.components[i].name
|
||
<< ": " << ModelID.RF.components[i].uuid << std::endl;
|
||
}
|
||
|
||
// RH
|
||
std::cout << " RH [" << ModelID.RH.components.size() << " components]:" << std::endl;
|
||
for (size_t i = 0; i < ModelID.RH.components.size(); ++i)
|
||
{
|
||
std::cout << " [" << i << "] " << ModelID.RH.components[i].name
|
||
<< ": " << ModelID.RH.components[i].uuid << std::endl;
|
||
}
|
||
|
||
// LegParameters
|
||
std::cout << "\n=== Param (LegParameters) ===" << std::endl;
|
||
|
||
std::cout << " LF:" << std::endl;
|
||
std::cout << " thigh_angle_deg: " << Param.LF.thigh_angle_deg << std::endl;
|
||
std::cout << " shank_angle_deg: " << Param.LF.shank_angle_deg << std::endl;
|
||
std::cout << " ankle_angle_deg: " << Param.LF.ankle_angle_deg << std::endl;
|
||
|
||
std::cout << " LH:" << std::endl;
|
||
std::cout << " thigh_angle_deg: " << Param.LH.thigh_angle_deg << std::endl;
|
||
std::cout << " shank_angle_deg: " << Param.LH.shank_angle_deg << std::endl;
|
||
std::cout << " ankle_angle_deg: " << Param.LH.ankle_angle_deg << std::endl;
|
||
|
||
std::cout << " RF:" << std::endl;
|
||
std::cout << " thigh_angle_deg: " << Param.RF.thigh_angle_deg << std::endl;
|
||
std::cout << " shank_angle_deg: " << Param.RF.shank_angle_deg << std::endl;
|
||
std::cout << " ankle_angle_deg: " << Param.RF.ankle_angle_deg << std::endl;
|
||
|
||
std::cout << " RH:" << std::endl;
|
||
std::cout << " thigh_angle_deg: " << Param.RH.thigh_angle_deg << std::endl;
|
||
std::cout << " shank_angle_deg: " << Param.RH.shank_angle_deg << std::endl;
|
||
std::cout << " ankle_angle_deg: " << Param.RH.ankle_angle_deg << std::endl;
|
||
|
||
// RobotBody
|
||
std::cout << "\n=== RobotBody ===" << std::endl;
|
||
std::cout << " BodyCode: " << RobotBody.BodyCode << std::endl;
|
||
std::cout << " x: " << RobotBody.x << std::endl;
|
||
std::cout << " y: " << RobotBody.y << std::endl;
|
||
std::cout << " z: " << RobotBody.z << std::endl;
|
||
std::cout << " qx: " << RobotBody.qx << std::endl;
|
||
std::cout << " qy: " << RobotBody.qy << std::endl;
|
||
std::cout << " qz: " << RobotBody.qz << std::endl;
|
||
std::cout << " qw: " << RobotBody.qw << std::endl;
|
||
|
||
// Other parameters
|
||
std::cout << "\n=== Other Parameters ===" << std::endl;
|
||
std::cout << " t_percentage: " << t_percentage << std::endl;
|
||
std::cout << " RobotID: " << RobotID << std::endl;
|
||
|
||
std::cout << "========================================" << std::endl;
|
||
std::cout << "END DEBUG PRINT" << std::endl;
|
||
std::cout << "========================================" << std::endl;
|
||
}
|
||
|
||
// RequestParameters 成员函数实现
|
||
RequestParameters RequestParameters::fromJsonString(const std::string &jsonStr)
|
||
{
|
||
try
|
||
{
|
||
auto j = nlohmann::json::parse(jsonStr);
|
||
|
||
// 如果不包含req_param，尝试直接解析
|
||
if (j.contains("GaitInfo") || j.contains("SystemParameters") ||
|
||
j.contains("ModelID") || j.contains("Param") || j.contains("RobotBody") || j.contains("t_percentage") || j.contains("RobotID"))
|
||
{
|
||
// 看起来是直接RequestParameters格式
|
||
return j.get<RequestParameters>();
|
||
}
|
||
else
|
||
{
|
||
|
||
if (j.contains("req_param") && !j["req_param"].is_null())
|
||
{
|
||
// req_param是一个嵌套的JSON对象，直接获取它
|
||
auto req_param_json = j["req_param"];
|
||
if (req_param_json.contains("GaitInfo") || req_param_json.contains("SystemParameters") ||
|
||
req_param_json.contains("ModelID") || req_param_json.contains("Param") || req_param_json.contains("RobotBody") || req_param_json.contains("t_percentage") || req_param_json.contains("RobotID"))
|
||
{
|
||
// 看起来是直接RequestParameters格式
|
||
return req_param_json.get<RequestParameters>();
|
||
}
|
||
}
|
||
}
|
||
|
||
// 返回默认对象
|
||
return RequestParameters();
|
||
}
|
||
catch (const nlohmann::json::exception &e)
|
||
{
|
||
std::cerr << "[ERROR] JSON解析失败: " << e.what() << std::endl;
|
||
return RequestParameters(); // 返回默认对象
|
||
}
|
||
catch (...)
|
||
{
|
||
std::cerr << "[ERROR] 未知错误解析JSON" << std::endl;
|
||
return RequestParameters(); // 返回默认对象
|
||
}
|
||
}
|
||
|
||
RequestParameters RequestParameters::fromJsonFile(const std::string &filename)
|
||
{
|
||
std::ifstream file(filename);
|
||
if (!file.is_open())
|
||
{
|
||
throw std::runtime_error("Cannot open file: " + filename);
|
||
}
|
||
|
||
std::stringstream buffer;
|
||
buffer << file.rdbuf();
|
||
return fromJsonString(buffer.str());
|
||
}
|
||
|
||
std::string RequestParameters::toJsonString() const
|
||
{
|
||
nlohmann::json j = *this;
|
||
return j.dump(2); // 缩进2个空格
|
||
}
|
||
|
||
bool RequestParameters::saveToFile(const std::string &filename) const
|
||
{
|
||
try
|
||
{
|
||
std::ofstream file(filename);
|
||
if (!file.is_open())
|
||
{
|
||
return false;
|
||
}
|
||
file << toJsonString();
|
||
return true;
|
||
}
|
||
catch (...)
|
||
{
|
||
return false;
|
||
}
|
||
}
|