std::map<K, V> checkGet(id<std::map<K, V>>, lua_State* state, int idx = -1)
{
if(!lua_istable(state, idx))
{
std::string msg = "Value at " + std::to_string(idx) + " of stack is not a table";
luaL_error(state, msg.c_str());
}
std::map<K, V> newMap;
// give lua_next nil to get the first key
lua_pushnil(state);
// "idx - 1" because the key is above the table
while(lua_next(state, idx - 1)) // pushes key, then value
{
// copy the key so it doesn't get popped
lua_pushvalue(state, -2);
K key = checkGet(id<K>{}, state);
V val = checkGet(id<V>{}, state, -2);
newMap.emplace(key, val);
lua_pop(state, 2); // pop key and value
}
lua_pop(state, 1); // pop the last key
return newMap;
}
void constDecl() /* 定数宣言のコンパイル */
{
Token temp;
while(1){
if (token.kind==Id){
setIdKind(constId); /* 印字のための情報のセット */
temp = token; /* 名前を入れておく */
token = checkGet(nextToken(), Equal); /* 名前の次は"="のはず */
if (token.kind==Num)
enterTconst(temp.u.id, token.u.value); /* 定数名と値をテーブルに */
else
errorType("number");
token = nextToken();
}else
errorMissingId();
if (token.kind!=Comma){ /* 次がコンマなら定数宣言が続く */
if (token.kind==Id){ /* 次が名前ならコンマを忘れたことにする */
errorInsert(Comma);
continue;
}else
break;
}
token = nextToken();
}
token = checkGet(token, Semicolon); /* 最後は";"のはず */
}
void factor() /* 式の因子のコンパイル */
{
int tIndex, i;
KeyId k;
if (token.kind==Id){
tIndex = searchT(token.u.id, varId);
setIdKind(k=kindT(tIndex)); /* 印字のための情報のセット */
switch (k) {
case varId: case parId: /* 変数名かパラメタ名 */
genCodeT(lod, tIndex);
token = nextToken(); break;
case constId: /* 定数名 */
genCodeV(lit, val(tIndex));
token = nextToken(); break;
case funcId: /* 関数呼び出し */
token = nextToken();
if (token.kind==Lparen){
i=0; /* iは実引数の個数 */
token = nextToken();
if (token.kind != Rparen) {
for (; ; ) {
expression(); i++; /* 実引数のコンパイル */
if (token.kind==Comma){ /* 次がコンマなら実引数が続く */
token = nextToken();
continue;
}
token = checkGet(token, Rparen);
break;
}
} else
token = nextToken();
if (pars(tIndex) != i)
errorMessage("\\#par"); /* pars(tIndex)は仮引数の個数 */
}else{
errorInsert(Lparen);
errorInsert(Rparen);
}
genCodeT(cal, tIndex); /* call命令 */
break;
}
}else if (token.kind==Num){ /* 定数 */
genCodeV(lit, token.u.value);
token = nextToken();
}else if (token.kind==Lparen){ /* 「(」「因子」「)」 */
token = nextToken();
expression();
token = checkGet(token, Rparen);
}
switch (token.kind){ /* 因子の後がまた因子ならエラー */
case Id: case Num: case Lparen:
errorMissingOp();
factor();
default:
return;
}
}
std::vector<T> checkGet(id<std::vector<T>>, lua_State* state, int idx = -1)
{
if(!lua_istable(state, idx))
{
std::string msg = "Value at " + std::to_string(idx) + " of stack is not a table";
luaL_error(state, msg.c_str());
}
std::vector<T> newVec;
#if LUA_VERSION_NUM >= 502
std::size_t tableLength = lua_rawlen(state, idx);
#else
std::size_t tableLength = lua_objlen(state, idx);
#endif
for(unsigned int i = 1; i <= tableLength; i++)
{
lua_rawgeti(state, idx, i);
newVec.push_back(checkGet(id<T>{}, state));
lua_pop(state, 1); // pop the pushed value
}
return newVec;
}
LRESULT ModelTreeDialog::doLineCheck(UINT checkId, LDLLineType lineType)
{
if (m_modelTree)
{
m_modelTree->setShowLineType(lineType, checkGet(checkId));
refreshTreeView();
}
return 0;
}
LRESULT ModelTreeDialog::doHighlightCheck(void)
{
m_highlight = checkGet(IDC_HIGHLIGHT);
if (m_highlight)
{
highlightItem(TreeView_GetSelection(m_hTreeView));
}
else
{
m_modelWindow->getModelViewer()->setHighlightPaths("");
}
TCUserDefaults::setBoolForKey(m_highlight, MODEL_TREE_HIGHLIGHT_KEY, false);
return 0;
}
void funcDecl() /* 関数宣言のコンパイル */
{
int fIndex;
if (token.kind==Id){
setIdKind(funcId); /* 印字のための情報のセット */
fIndex = enterTfunc(token.u.id, nextCode()); /* 関数名をテーブルに登録 */
/* その先頭番地は、まず、次のコードの番地nextCode()とする */
token = checkGet(nextToken(), Lparen);
blockBegin(FIRSTADDR); /* パラメタ名のレベルは関数のブロックと同じ */
while(1){
if (token.kind==Id){ /* パラメタ名がある場合 */
setIdKind(parId); /* 印字のための情報のセット */
enterTpar(token.u.id); /* パラメタ名をテーブルに登録 */
token = nextToken();
}else
break;
if (token.kind!=Comma){ /* 次がコンマならパラメタ名が続く */
if (token.kind==Id){ /* 次が名前ならコンマを忘れたことに */
errorInsert(Comma);
continue;
}else
break;
}
token = nextToken();
}
token = checkGet(token, Rparen); /* 最後は")"のはず */
endpar(); /* パラメタ部が終わったことをテーブルに連絡 */
if (token.kind==Semicolon){
errorDelete();
token = nextToken();
}
block(fIndex); /* ブロックのコンパイル、その関数名のインデックスを渡す */
token = checkGet(token, Semicolon); /* 最後は";"のはず */
} else
errorMissingId(); /* 関数名がない */
}
void varDecl() /* 変数宣言のコンパイル */
{
while(1){
if (token.kind==Id){
setIdKind(varId); /* 印字のための情報のセット */
enterTvar(token.u.id); /* 変数名をテーブルに、番地はtableが決める */
token = nextToken();
}else
errorMissingId();
if (token.kind!=Comma){ /* 次がコンマなら変数宣言が続く */
if (token.kind==Id){ /* 次が名前ならコンマを忘れたことにする */
errorInsert(Comma);
continue;
}else
break;
}
token = nextToken();
}
token = checkGet(token, Semicolon); /* 最後は";"のはず */
}
void JpegOptionsDialog::doOK(void)
{
TCJpegOptions::SubSampling subSampling = TCJpegOptions::SS444;
switch (comboGetCurSel(IDC_JPEG_SUBSAMPLING_COMBO))
{
case 0:
subSampling = TCJpegOptions::SS444;
break;
case 1:
subSampling = TCJpegOptions::SS422;
break;
case 2:
subSampling = TCJpegOptions::SS420;
break;
}
options->setQuality(trackBarGetPos(IDC_JPEG_QUAL_SLIDER));
options->setSubSampling(subSampling);
options->setProgressive(checkGet(IDC_JPEG_PROGRESSIVE_CHECK));
options->save();
CUIDialog::doOK();
}
FDBinary::time_type FDBinary::getTime64() {
checkGet(8);
uint8_t *p = &buffer[getIndex];
getIndex += 8;
return FDBinary::unpackTime64(p);
}
std::vector<uint8_t> FDBinary::getData(std::size_t length) {
checkGet(length);
std::vector<uint8_t> data = std::vector<uint8_t>(buffer.begin() + getIndex, buffer.begin() + getIndex + length);
getIndex += length;
return data;
}
uint8_t FDBinary::getUInt8() {
checkGet(1);
uint8_t *p = &buffer[getIndex];
getIndex += 1;
return FDBinary::unpackUInt8(p);
}
void statement() /* 文のコンパイル */
{
int tIndex;
KindT k;
int backP, backP2; /* バックパッチ用 */
while(1) {
switch (token.kind) {
case Id: /* 代入文のコンパイル */
tIndex = searchT(token.u.id, varId); /* 左辺の変数のインデックス */
setIdKind(k=kindT(tIndex)); /* 印字のための情報のセット */
if (k != varId && k != parId) /* 変数名かパラメタ名のはず */
errorType("var/par");
token = checkGet(nextToken(), Assign); /* ":="のはず */
expression(); /* 式のコンパイル */
genCodeT(sto, tIndex); /* 左辺への代入命令 */
return;
case If: /* if文のコンパイル */
token = nextToken();
condition(); /* 条件式のコンパイル */
token = checkGet(token, Then); /* "then"のはず */
backP = genCodeV(jpc, 0); /* jpc命令 */
statement(); /* 文のコンパイル */
backPatch(backP); /* 上のjpc命令にバックパッチ */
return;
case Ret: /* return文のコンパイル */
token = nextToken();
expression(); /* 式のコンパイル */
genCodeR(); /* ret命令 */
return;
case Begin: /* begin . . end文のコンパイル */
token = nextToken();
while(1){
statement(); /* 文のコンパイル */
while(1){
if (token.kind==Semicolon){ /* 次が";"なら文が続く */
token = nextToken();
break;
}
if (token.kind==End){ /* 次がendなら終り */
token = nextToken();
return;
}
if (isStBeginKey(token)){ /* 次が文の先頭記号なら */
errorInsert(Semicolon); /* ";"を忘れたことにする */
break;
}
errorDelete(); /* それ以外ならエラーとして読み捨てる */
token = nextToken();
}
}
case While: /* while文のコンパイル */
token = nextToken();
backP2 = nextCode(); /* while文の最後のjmp命令の飛び先 */
condition(); /* 条件式のコンパイル */
token = checkGet(token, Do); /* "do"のはず */
backP = genCodeV(jpc, 0); /* 条件式が偽のとき飛び出すjpc命令 */
statement(); /* 文のコンパイル */
genCodeV(jmp, backP2); /* while文の先頭へのジャンプ命令 */
backPatch(backP); /* 偽のとき飛び出すjpc命令へのバックパッチ */
return;
case Write: /* write文のコンパイル */
token = nextToken();
expression(); /* 式のコンパイル */
genCodeO(wrt); /* その値を出力するwrt命令 */
return;
case WriteLn: /* writeln文のコンパイル */
token = nextToken();
genCodeO(wrl); /* 改行を出力するwrl命令 */
return;
case End: case Semicolon: /* 空文を読んだことにして終り */
return;
default: /* 文の先頭のキーまで読み捨てる */
errorDelete(); /* 今読んだトークンを読み捨てる */
token = nextToken();
continue;
}
}
}
uint16_t FDBinary::getUInt16() {
checkGet(2);
uint8_t *p = &buffer[getIndex];
getIndex += 2;
return FDBinary::unpackUInt16(p);
}
float FDBinary::getFloat32() {
checkGet(4);
uint8_t *p = &buffer[getIndex];
getIndex += 4;
return FDBinary::unpackFloat32(p);
}
float FDBinary::getFloat16() {
checkGet(2);
uint8_t *p = &buffer[getIndex];
getIndex += 2;
return FDBinary::unpackFloat16(p);
}
uint64_t FDBinary::getUInt64() {
checkGet(8);
uint8_t *p = &buffer[getIndex];
getIndex += 8;
return FDBinary::unpackUInt64(p);
}
uint32_t FDBinary::getUInt32() {
checkGet(4);
uint8_t *p = &buffer[getIndex];
getIndex += 4;
return FDBinary::unpackUInt32(p);
}
std::tuple<T...> getTuple(lua_State* state, indices<N...>)
{
constexpr int size = static_cast<int>(sizeof...(T));
(void)state; // friggin weird error about "state being unused"
return std::make_tuple(checkGet(id<T>{}, state, N - size)...);
}
void smurf::Robot::loadFromSmurf(const std::string& path)
{
configmaps::ConfigMap map;
// parse joints from model
boost::filesystem::path filepath(path);
model = smurf_parser::parseFile(&map, filepath.parent_path().generic_string(), filepath.filename().generic_string(), true);
//first we need to create all Frames
for (configmaps::ConfigVector::iterator it = map["frames"].begin(); it != map["frames"].end(); ++it)
{
configmaps::ConfigMap &fr(it->children);
Frame *frame = new Frame(fr["name"]);
availableFrames.push_back(frame);
//std::cout << "Adding additional frame " << frame->getName() << std::endl;
}
for(std::pair<std::string, boost::shared_ptr<urdf::Link>> link: model->links_)
{
Frame *frame = new Frame(link.first);
for(boost::shared_ptr<urdf::Visual> visual : link.second->visual_array)
{
frame->addVisual(*visual);
}
availableFrames.push_back(frame);
//std::cout << "Adding link frame " << frame->getName() << std::endl;
}
for(std::pair<std::string, boost::shared_ptr<urdf::Joint> > jointIt: model->joints_)
{
boost::shared_ptr<urdf::Joint> joint = jointIt.second;
//std::cout << "Adding joint " << joint->name << std::endl;
Frame *source = getFrameByName(joint->parent_link_name);
Frame *target = getFrameByName(joint->child_link_name);
//TODO this might not be set in some cases, perhaps force a check
configmaps::ConfigMap annotations;
for(configmaps::ConfigItem &cv : map["joints"])
{
if(static_cast<std::string>(cv.children["name"]) == joint->name)
{
annotations = cv.children;
}
}
switch(joint->type)
{
case urdf::Joint::FIXED:
{
const urdf::Pose &tr(joint->parent_to_joint_origin_transform);
StaticTransformation *transform = new StaticTransformation(source, target,
Eigen::Quaterniond(tr.rotation.w, tr.rotation.x, tr.rotation.y, tr.rotation.z),
Eigen::Vector3d(tr.position.x, tr.position.y, tr.position.z));
staticTransforms.push_back(transform);
}
break;
case urdf::Joint::FLOATING:
{
DynamicTransformation *transform = new DynamicTransformation(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"));
dynamicTransforms.push_back(transform);
Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
sourceToAxis.translation() = axis;
base::JointLimitRange limits;
const urdf::Pose parentToOrigin(joint->parent_to_joint_origin_transform);
Eigen::Quaterniond rot(parentToOrigin.rotation.w, parentToOrigin.rotation.x, parentToOrigin.rotation.y, parentToOrigin.rotation.z);
Eigen::Vector3d trans(parentToOrigin.position.x, parentToOrigin.position.y, parentToOrigin.position.z);
Eigen::Affine3d parentToOriginAff;
parentToOriginAff.setIdentity();
parentToOriginAff.rotate(rot);
parentToOriginAff.translation() = trans;
Joint *smurfJoint = new Joint (source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, parentToOriginAff, joint);
joints.push_back(smurfJoint);
}
break;
case urdf::Joint::REVOLUTE:
case urdf::Joint::CONTINUOUS:
case urdf::Joint::PRISMATIC:
{
base::JointState minState;
minState.position = joint->limits->lower;
minState.effort = 0;
minState.speed = 0;
base::JointState maxState;
maxState.position = joint->limits->upper;
maxState.effort = joint->limits->effort;
maxState.speed = joint->limits->velocity;
base::JointLimitRange limits;
limits.min = minState;
limits.max = maxState;
Eigen::Vector3d axis(joint->axis.x, joint->axis.y, joint->axis.z);
Eigen::Affine3d sourceToAxis(Eigen::Affine3d::Identity());
sourceToAxis.translation() = axis;
DynamicTransformation *transform = NULL;
Joint *smurfJoint;
// push the correspondent smurf::joint
const urdf::Pose parentToOrigin(joint->parent_to_joint_origin_transform);
Eigen::Quaterniond rot(parentToOrigin.rotation.w, parentToOrigin.rotation.x, parentToOrigin.rotation.y, parentToOrigin.rotation.z);
Eigen::Vector3d trans(parentToOrigin.position.x, parentToOrigin.position.y, parentToOrigin.position.z);
Eigen::Affine3d parentToOriginAff;
parentToOriginAff.setIdentity();
parentToOriginAff.rotate(rot);
parentToOriginAff.translation() = trans;
if(joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::CONTINUOUS)
{
transform = new RotationalJoint(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, axis, parentToOriginAff, joint);
smurfJoint = (Joint *)transform;
}
else
{
transform = new TranslationalJoint(source, target, checkGet(annotations, "provider"), checkGet(annotations, "port"), checkGet(annotations, "driver"), limits, sourceToAxis, axis, parentToOriginAff, joint);
smurfJoint = (Joint *)transform;
}
dynamicTransforms.push_back(transform);
joints.push_back(smurfJoint);
}
break;
default:
throw std::runtime_error("Smurf: Error, got unhandles Joint type");
}
}
// parse sensors from map
for (configmaps::ConfigVector::iterator it = map["sensors"].begin(); it != map["sensors"].end(); ++it)
{
configmaps::ConfigMap sensorMap = it->children;
smurf::Sensor *sensor = new Sensor(sensorMap["name"], sensorMap["type"], sensorMap["taskInstanceName"], getFrameByName(sensorMap["link"]));
sensors.push_back(sensor);
}
}
