Aris::Core::MSG parsePre2Shv(const std::string &cmd, const map<std::string, std::string> ¶ms)
{
PRE2SHV_PARAM param;
for(auto &i:params)
{
if(i.first=="distance")
{
param.distance=stod(i.second);
}
else if(i.first=="height")
{
param.height=stod(i.second);
}
else
{
std::cout<<"parse failed"<<std::endl;
return MSG{};
}
}
param.periodCount[0]=3000;
param.periodCount[1]=3000;
Aris::Core::MSG msg;
msg.CopyStruct(param);
std::cout<<"finished parse"<<std::endl;
return msg;
}
Aris::Core::MSG parseSwing(const std::string &cmd, const map<std::string, std::string> ¶ms)
{
SWING_PARAM param;
for(auto &i:params)
{
if(i.first=="y")
{
param.centreP[1]=stod(i.second);
}
else if(i.first=="z")
{
param.centreP[2]=stod(i.second);
}
else if(i.first=="deg")
{
param.swingRad=stod(i.second)/180*PI;//计算身体摆动的弧度
}
else
{
std::cout<<"parse failed"<<std::endl;
return MSG{};
}
}
param.periodCount=3000;
Aris::Core::MSG msg;
msg.CopyStruct(param);
std::cout<<"finished parse"<<std::endl;
return msg;
}
Aris::Core::MSG parseShovel(const std::string &cmd, const map<std::string, std::string> ¶ms)
{
SHOVEL_PARAM param;
param.radius=2;
param.periodNum = 7;
for(int i = 0; i < param.periodNum; i++)
{
param.periodCount[i]=3000;
}
for(auto &i:params)
{
if(i.first=="height")
{
param.height=stod(i.second);
}
else if(i.first=="length")
{
param.length=stod(i.second);
}
else if(i.first=="degree")
{
param.rad=stod(i.second)/180*PI;
}
else
{
std::cout<<"parse failed"<<std::endl;
return MSG{};
}
}
Aris::Core::MSG msg;
msg.CopyStruct(param);
std::cout<<"finished parse"<<std::endl;
return msg;
}
Aris::Core::MSG parseCWF(const std::string &cmd, const std::map<std::string, std::string> ¶ms)
{
Robots::WALK_PARAM param;
for (auto &i : params)
{
if (i.first == "totalCount")
{
param.totalCount = std::stoi(i.second);
}
else if (i.first == "walkDirection")
{
param.walkDirection = std::stoi(i.second);
}
else if (i.first == "upDirection")
{
param.upDirection = std::stoi(i.second);
}
else if (i.first == "distance")
{
param.d = std::stod(i.second);
}
else if (i.first == "height")
{
param.h = std::stod(i.second);
}
}
isStoppingCWF=false;
Aris::Core::MSG msg;
msg.CopyStruct(param);
return msg;
}
Aris::Core::MSG parseMove2(const std::string &cmd, const map<std::string, std::string> ¶ms)
{
MOVES_PARAM param;
double targetPos[3]; //移动目标位置
for(auto &i:params)
{
if(i.first=="component")
{
if(i.second=="lf")
{
param.comID=0;
}
else if(i.second=="lm")
{
param.comID=1;
}
else if(i.second=="lr")
{
param.comID=2;
}
else if(i.second=="rf")
{
param.comID=3;
}
else if(i.second=="rm")
{
param.comID=4;
}
else if(i.second=="rr")
{
param.comID=5;
}
else if(i.second=="bd")
{
param.comID=6;
}
else
{
std::cout<<"parse failed"<<std::endl;
return MSG{};
}
}
//绝对坐标移动
else if(i.first=="x")
{
targetPos[0]=stod(i.second);
param.isAbsolute=true;
}
else if(i.first=="y")
{
targetPos[1]=stod(i.second);
param.isAbsolute=true;
}
else if(i.first=="z")
{
targetPos[2]=stod(i.second);
param.isAbsolute=true;
}
//相对坐标移动
else if(i.first=="u")
{
targetPos[0]=stod(i.second);
}
else if(i.first=="v")
{
targetPos[1]=stod(i.second);
}
else if(i.first=="w")
{
targetPos[2]=stod(i.second);
}
else
{
std::cout<<"parse failed"<<std::endl;
return MSG{};
}
}
if(param.comID==6)
{
std::copy_n(targetPos, 3, param.targetBodyPE);
}
else
{
std::copy_n(targetPos, 3, ¶m.targetPee[3*param.comID]);
param.legNum=1;
param.motorNum=3;
param.legID[0]=param.comID;
int motors[3] = { 3*param.comID, 3*param.comID+1, 3*param.comID+2 };
std::copy_n(motors, 9, param.motorID);
}
param.periodCount=3000;
Aris::Core::MSG msg;
msg.CopyStruct(param);
std::cout<<"finished parse"<<std::endl;
return msg;
}