// foreachloop
static inline void _vm_foreachloop(struct a2_vm* vm_p){
struct a2_obj* _k = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
struct a2_obj* _v = callinfo_sfreg(curr_ci, ir_ga(curr_ir)+1);
struct a2_obj* _c = callinfo_sfreg(curr_ci, ir_ga(curr_ir)+2);
struct a2_obj* __v = NULL;
if(obj_t(_c)!=A2_TMAP && obj_t(_c)!=A2_TARRAY)
vm_error("the varable is not map or array.");
// dump next
switch(obj_t(_c)){
case A2_TMAP:
__v = a2_map_next(a2_gcobj2map(obj_vX(_c, obj)), _k);
if(__v==NULL)
curr_pc++;
else{
*_v = *__v;
jump(ir_gbx(curr_ir));
}
break;
case A2_TARRAY:
__v = a2_array_next(a2_gcobj2array(obj_vX(_c, obj)), _k);
if(__v==NULL) // dump is end
curr_pc++;
else{
*_v = *__v;
jump(ir_gbx(curr_ir));
}
break;
default:
assert(0);
}
}
// test
static inline void _vm_test(struct a2_vm* vm_p){
struct a2_obj* _v = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
if(obj_t(_v)==A2_TNIL || (obj_t(_v)==A2_TBOOL && !(obj_vX(_v, uinteger))))
jump(ir_gbx(curr_ir));
else
curr_pc++;
}
// set value
static inline void _vm_setvalue(struct a2_vm* vm_p){
struct a2_obj* _c = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
struct a2_obj* _k = _getvalue(vm_p, ir_gb(curr_ir));
struct a2_obj* _v = _getvalue(vm_p, ir_gc(curr_ir));
struct a2_obj* __d = NULL;
switch(obj_t(_c)){
case A2_TARRAY:
if(obj_t(_k)!=A2_TNUMBER)
vm_error("the key is must number at set array.");
__d = a2_array_get(a2_gcobj2array(obj_vX(_c, obj)), _k);
if(!__d) goto SVALUE_ERROR;
*__d = *_v;
break;
case A2_TMAP:
if(obj_t(_k)!=A2_TNUMBER && obj_t(_k)!=A2_TSTRING)
vm_error("the key is must number or string at set map.");
__d = a2_map_query(a2_gcobj2map(obj_vX(_c, obj)), _k);
if(!__d) goto SVALUE_ERROR;
*__d = *_v;
break;
default:
vm_error("the varable is not map or array.");
}
curr_pc++;
return;
SVALUE_ERROR:
vm_error("the key is overfllow.");
}
A2_API void a2_setmeta(struct a2_state* state){
int top = a2_top(state)-1;
struct a2_obj* m = a2_getcstack(state->env_p, top);
struct a2_obj* v = a2_getcstack(state->env_p, top-1);
if(obj_t(v)!=A2_TMAP || obj_t(m)!=A2_TMAP)
a2_err(state, "the value and meta should map type.");
a2_gc_setmeta(obj_vX(v, obj), obj_vX(m, obj));
a2_topset(state, top-1);
}
// forloop
static inline void _vm_forloop(struct a2_vm* vm_p){
struct a2_obj* _i = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
struct a2_obj* _count = callinfo_sfreg(curr_ci, ir_ga(curr_ir)+1);
struct a2_obj* _step = callinfo_sfreg(curr_ci, ir_ga(curr_ir)+2);
if(obj_t(_i)!=A2_TNUMBER)
vm_error("the index varable is must number.");
assert(obj_t(_i)==A2_TNUMBER && obj_t(_count)==A2_TNUMBER && obj_t(_step)==A2_TNUMBER);
obj_vNum(_i) += obj_vNum(_step);
if(obj_vNum(_i) < obj_vNum(_count)) // continue for
jump(ir_gbx(curr_ir));
else // for end
curr_pc++;
}
A2_API inline a2_number a2_tonumber(struct a2_state* state, int idx){
struct a2_obj* obj = a2_getcstack(state->env_p, idx);
if(obj_t(obj)!=A2_TNUMBER)
return 0.0;
else
return obj_vNum(obj);
}
A2_API inline char* a2_tostring(struct a2_state* state, int idx){
struct a2_obj* obj = a2_getcstack(state->env_p, idx);
if(obj_t(obj)!=A2_TSTRING)
return "";
else
return a2_gcobj2string(obj_vX(obj, obj));
}
A2_API inline a2_cfunction a2_tocfunction(struct a2_state* state, int idx){
struct a2_obj* obj = a2_getcstack(state->env_p, idx);
if(obj_t(obj)!=A2_TCFUNCTION)
return NULL;
else
return obj_vX(obj, cfunction);
}
// mark closure
static inline void _gc_mark_closure(struct a2_gc* gc_p, struct a2_gcobj* gcobj, enum gc_mark m){
assert(gcobj->type == A2_TCLOSURE);
if(mask(gcobj->mark) == mask(m))
return;
gcobj->mark = m;
size_t i;
struct a2_closure* cls = a2_gcobj2closure(gcobj);
// mark upvalue
for(i=0; i<cls->uv_size; i++){
if(cls->uv_chain[i].type == uv_gc){
cls->uv_chain[i].v.uv_obj->mark = m;
assert(cls->uv_chain[i].v.uv_obj->type == _A2_TUPVALUE);
a2_gc_markit(gc_p, cls->uv_chain[i].v.uv_obj->value.uv, m);
}
}
// mark const varable
for(i=0; i<cls->xcls_p->c_stack.top; i++){
if(obj_t(&(cls->xcls_p->c_stack.stk_p[i])) == A2_TSTRING){
obj_vX(&(cls->xcls_p->c_stack.stk_p[i]), obj)->mark = m;
}
}
}
inline char* obj2str(struct a2_obj* obj, char* buf, size_t len){
assert(obj);
switch(obj_t(obj)){
case A2_TNUMBER:
snprintf(buf, len, "%.14g", obj_vNum(obj));
return buf;
case A2_TSTRING:
return a2_gcobj2string(obj_vX(obj, obj));
case A2_TBOOL:
snprintf(buf, len, "%s", (obj_vX(obj, uinteger))?("true"):("false"));
return buf;
case A2_TNIL:
return "nil";
case _A2_TADDR:
_sf(buf, len, "[%zd]", obj_vX(obj, addr));
return buf;
case A2_TCLOSURE:
snprintf(buf, len, "closure:%p", a2_gcobj2closure(obj_vX(obj, obj)));
return buf;
case A2_TARRAY:
snprintf(buf, len, "array:%p", a2_gcobj2array(obj_vX(obj, obj)));
return buf;
case A2_TMAP:
snprintf(buf, len, "map:%p", a2_gcobj2map(obj_vX(obj, obj)));
return buf;
default:
assert(0);
}
return NULL;
}
// forprep
static inline void _vm_forprep(struct a2_vm* vm_p){
struct a2_obj* _i = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
struct a2_obj* _count = callinfo_sfreg(curr_ci, ir_ga(curr_ir)+1);
struct a2_obj* _step = callinfo_sfreg(curr_ci, ir_ga(curr_ir)+2);
if(obj_t(_i)!=A2_TNUMBER)
vm_error("the index varable is must number.");
if(obj_t(_count)!=A2_TNUMBER)
vm_error("the for's count is must number.");
if(obj_t(_step)!=A2_TNUMBER)
vm_error("the for's step is must number.");
if(obj_vNum(_i)>=obj_vNum(_count)) // for end
jump(ir_gbx(curr_ir));
else // for continue
curr_pc++;
}
inline struct a2_obj* a2_array_get(struct a2_array* array_p, struct a2_obj* k){
assert(obj_t(k)==A2_TNUMBER);
size_t idx=(size_t)(obj_vNum(k));
if(idx>=array_p->len)
return NULL;
return &(array_p->list[idx]);
}
// neg
static inline void _vm_neg(struct a2_vm* vm_p){
struct a2_obj* _d = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
struct a2_obj* _v = _getvalue(vm_p, ir_gbx(curr_ir));
if(obj_t(_v)!=A2_TNUMBER)
vm_error("the varable is not number type at set neg.");
*_d = a2_number2obj(obj_vNum(_v)*(-1));
curr_pc++;
}
//set list
static inline void _vm_setlist(struct a2_vm* vm_p){
int i, end=ir_gb(curr_ir)+ir_gc(curr_ir);
struct a2_obj* _d = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
assert(obj_t(_d)==A2_TARRAY);
for(i=ir_gb(curr_ir); i<end; i++){
a2_array_add(a2_gcobj2array(obj_vX(_d, obj)), callinfo_sfreg(curr_ci, i));
}
curr_pc++;
}
A2_API inline int a2_tobool(struct a2_state* state, int idx){
struct a2_obj* obj = a2_getcstack(state->env_p, idx);
if(obj_t(obj)!=A2_TBOOL)
a2_error(state->env_p, e_run_error,
"the type is not bool.");
else
return (int)(obj_vX(obj, uinteger));
return 0;
}
A2_API int a2_pcall(struct a2_state* state, int args){
struct a2_obj* cls_obj = a2_getcstack(state->env_p, a2_top(state)-args-1);
struct a2_obj* args_obj = a2_getcstack(state->env_p, a2_top(state)-args);
if(obj_t(cls_obj) !=A2_TCLOSURE)
a2_error(state->env_p, e_run_error, "the type is not closure at pcall.\n");
return a2_vm_pcall(a2_envvm(state->env_p), cls_obj, args_obj, args);
}
/*
* returns:
* 0 - robot transform
* 1 - object transform
*/
std::vector<tf::Transform>
getObjectPose(const gazebo_msgs::ModelStates::ConstPtr & msg, std::string obj_name)
{
tf::Vector3 pose(0,0,0);
tf::Quaternion rot(0,0,0,1);
tf::Transform obj_t(rot, pose);
tf::Transform rob(rot, pose);
unsigned int found = 0;
std::vector<tf::Transform> tfs;
unsigned int count = msg->name.size();
// Note: backwards loop as typically these objects are at the end of the
// array. Does not have to be!
for (int i=count-1; i>=0; --i)
{
if( obj_name == msg->name[i] )
{
pose.setValue(
msg->pose[i].position.x,
msg->pose[i].position.y,
msg->pose[i].position.z);
rot.setX(msg->pose[i].orientation.x);
rot.setY(msg->pose[i].orientation.y);
rot.setZ(msg->pose[i].orientation.z);
rot.setW(msg->pose[i].orientation.w);
obj_t.setOrigin(pose);
obj_t.setRotation(rot);
++found;
}
if( robot_gaz_name == msg->name[i] )
{
pose.setValue(
msg->pose[i].position.x,
msg->pose[i].position.y,
msg->pose[i].position.z);
rot.setX(msg->pose[i].orientation.x);
rot.setY(msg->pose[i].orientation.y);
rot.setZ(msg->pose[i].orientation.z);
rot.setW(msg->pose[i].orientation.w);
rob.setOrigin(pose);
rob.setRotation(rot);
++found;
}
// optimization
if(found >= 2) break;
}
tfs.push_back(rob);
tfs.push_back(obj_t);
//ROS_INFO("found pose: %4.2f, %4.2f", msg->pose[i].position.x, msg->pose[i].position.y);
//ROS_INFO("found pose: %4.2f, %4.2f", obj_t.getOrigin().getX(), obj_t.getOrigin().getY());
return tfs;
}
inline struct a2_obj* a2_array_next(struct a2_array* array_p, struct a2_obj* k){
assert(k);
if(obj_t(k) == A2_TNIL){
obj_setNum(k, -1);
}
if( (size_t)(obj_vNum(k) + 1)>=array_p->len )
return NULL;
struct a2_obj* v = &(array_p->list[(size_t)(obj_vNum(k)+1)]);
a2_number number = (a2_number)((size_t)(obj_vNum(k))+1);
obj_setNum(k, number);
return v;
}
// call
static inline void _vm_call(struct a2_vm* vm_p){
struct a2_obj* _func = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
switch(obj_t(_func)){
case A2_TCLOSURE:
__vm_call_function(vm_p, _func);
break;
case A2_TCFUNCTION:
__vm_call_cfunction(vm_p, _func);
break;
default:
vm_error("the varable is not function type.");
}
}
A2_API inline void* a2_topoint(struct a2_state* state, int idx){
struct a2_obj* obj = a2_getcstack(state->env_p, idx);
switch(obj_t(obj)){
case A2_TARRAY:
return a2_gcobj2array(obj_vX(obj, obj));
case A2_TMAP:
return a2_gcobj2map(obj_vX(obj, obj));
case A2_TCLOSURE:
return a2_gcobj2closure(obj_vX(obj, obj));
default:
return NULL;
}
}
// test obj dump
void obj_dump(struct a2_obj* obj){
switch(obj_t(obj)){
case A2_TSTRING:
printf("%s", a2_gcobj2string(obj_vX(obj, obj)));
break;
case A2_TNUMBER:
printf("%.14g", obj_vNum(obj));
break;
default:
printf("<null>");
break;
}
}
// mark map
static inline void _gc_mark_map(struct a2_gc* gc_p, struct a2_gcobj* gcobj, enum gc_mark m){
assert(gcobj->type == A2_TMAP);
if(mask(gcobj->mark) == mask(m))
return;
gcobj->mark = m;
struct a2_obj k = a2_nil2obj();
struct a2_obj* vp = NULL;
struct a2_map* map_p = a2_gcobj2map(gcobj);
while(NULL != (vp = a2_map_next(map_p, &k))){
if(obj_t(&k)==A2_TSTRING){
gcobj->mark = m;
}
a2_gc_markit(gc_p, vp, m);
}
}
void a2_gc_markit(struct a2_gc* gc_p, struct a2_obj* obj, enum gc_mark m){
switch(obj_t(obj)){
case A2_TARRAY:
_gc_mark_array(gc_p, obj_vX(obj, obj), m);
break;
case A2_TMAP:
_gc_mark_map(gc_p, obj_vX(obj, obj), m);
break;
case A2_TSTRING:
obj_vX(obj, obj)->mark = m;
break;
case A2_TCLOSURE:
_gc_mark_closure(gc_p, obj_vX(obj, obj), m);
break;
}
}
// call a2 function
static inline void __vm_call_function(struct a2_vm* vm_p, struct a2_obj* _func){
assert(obj_t(_func)==A2_TCLOSURE);
struct a2_obj* _obj = NULL;
int i, j, params = a2_closure_params(a2_gcobj2closure(obj_vX(_func, obj)));
struct a2_array* _args = NULL;
struct vm_callinfo* _ci = curr_ci;
ir _ir = curr_ir;
// new call info
int b = ir_ga(curr_ir), n=ir_gc(curr_ir);
callinfo_new(vm_p, a2_gcobj2closure(obj_vX(_func, obj)), b, n);
// jump call
_ci->pc++;
// if is mutableargs
if(params<0){
params = -1 - params;
_obj = callinfo_sfreg(curr_ci, params);
if(ir_gb(_ir)>params){ // set _args list
_args = a2_array_new();
struct a2_gcobj* _array_gcobj = a2_array2gcobj(_args);
a2_gcadd(vm_p->env_p, _array_gcobj);
obj_setX(_obj, A2_TARRAY, obj, _array_gcobj);
}else{
obj_setX(_obj, A2_TNIL, point, NULL);
}
}
// set params
for(i=ir_ga(_ir)+1, j=0;
i<=ir_ga(_ir)+ir_gb(_ir) && j<params;
j++, i++){
_obj = callinfo_sfreg(curr_ci, j);
*_obj = *callinfo_sfreg(_ci, i);
}
// set clear params
for( ;j<params; j++){
_obj = callinfo_sfreg(curr_ci, j);
obj_setX(_obj, A2_TNIL, point, NULL);
}
// if mutable args
for(j=0; i<=ir_ga(_ir)+ir_gb(_ir) && _args; i++, j++){
a2_array_add(_args, callinfo_sfreg(_ci, i));
}
}
// not
static inline void _vm_not(struct a2_vm* vm_p){
struct a2_obj* _d = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
struct a2_obj* _v = _getvalue(vm_p, ir_gbx(curr_ir));
switch(obj_t(_v)){
case A2_TNIL:
*_d = a2_bool2obj(1);
break;
case A2_TBOOL:
*_d = a2_bool2obj(!(obj_vX(_v, uinteger)));
break;
default:
vm_error("the varable is not bool type.");
}
curr_pc++;
}
// get data size from a2_obj
size_t a2_obj_size(struct a2_obj* obj_p){
assert(obj_p);
switch(obj_t(obj_p)){
case A2_TSTRING:
return a2_string_len(a2_gcobj2string(obj_vX(obj_p, obj)));
case A2_TNUMBER:
return sizeof(a2_number);
case A2_TNIL:
return 0;
case A2_TBOOL:
return sizeof(obj_vX(obj_p, uinteger));
case _A2_TADDR:
return sizeof(obj_vX(obj_p, addr));
default:
assert(0);
return 0;
}
}
byte* a2_obj_bytes(struct a2_obj* obj_p){
assert(obj_p);
switch(obj_t(obj_p)){
case A2_TSTRING:
return (byte*)(a2_gcobj2string(obj_vX(obj_p, obj)));
case A2_TNUMBER:
return (byte*)(&obj_vNum(obj_p));
case A2_TNIL:
return NULL;
case A2_TBOOL:
return (byte*)(&obj_vX(obj_p, uinteger));
case _A2_TADDR:
return (byte*)(&obj_vX(obj_p, addr));
default:
assert(0);
return NULL;
}
}
//set map
static inline void _vm_setmap(struct a2_vm* vm_p){
int i, end=ir_gb(curr_ir)+2*ir_gc(curr_ir);
struct a2_obj* _d = callinfo_sfreg(curr_ci, ir_ga(curr_ir));
struct a2_obj* _v = NULL;
struct a2_map* map = NULL;
assert(obj_t(_d)==A2_TMAP);
struct a2_kv kv={0};
for(i=ir_gb(curr_ir); i<end; i+=2){
kv.key = callinfo_sfreg(curr_ci, i);
kv.vp = callinfo_sfreg(curr_ci, i+1);
map = a2_gcobj2map(obj_vX(_d, obj));
if( (_v=a2_map_query(map, kv.key))==NULL )
a2_map_add(map, &kv);
else
*_v = *kv.vp;
}
curr_pc++;
}
A2_API inline void a2_len(struct a2_state* state, int idx){
struct a2_obj* obj = a2_getcstack(state->env_p, idx);
size_t len =0;
struct a2_obj len_obj;
switch(obj_t(obj)){
case A2_TMAP:
len = a2_map_len(a2_gcobj2map(obj_vX(obj, obj)));
break;
case A2_TARRAY:
len = a2_array_len(a2_gcobj2array(obj_vX(obj, obj)));
break;
case A2_TSTRING:
len = a2_string_len(a2_gcobj2string(obj_vX(obj, obj)));
break;
default:
a2_error(state->env_p, e_run_error,
"the type is not map or array at len function.");
}
len_obj = a2_number2obj((a2_number)len);
a2_pushstack(state->env_p, &len_obj);
}
// call c function
static inline void __vm_call_cfunction(struct a2_vm* vm_p, struct a2_obj* _func){
assert(obj_t(_func)==A2_TCFUNCTION);
int i, j;
struct a2_obj* _obj = NULL;
// back bottom
int _b = a2_getbottom(vm_p->env_p);
int _top = a2_gettop(vm_p->env_p);
a2_setbottom(vm_p->env_p, a2_gettop(vm_p->env_p));
// closure arg to cstack
for(i=ir_ga(curr_ir)+1; i<=ir_ga(curr_ir)+ir_gb(curr_ir); i++){
_obj = callinfo_sfreg(curr_ci, i);
a2_pushstack(vm_p->env_p, _obj);
}
// call c function
callinfo_new(vm_p, NULL, 0, 0);
int ret = obj_vX(_func, cfunction)(a2_env2state(vm_p->env_p));
callinfo_free(vm_p);
int size = a2_gettop(vm_p->env_p)-a2_getbottom(vm_p->env_p);
// set return value
for(i=size-ret, j=ir_ga(curr_ir);
i<size && j<ir_ga(curr_ir)+ir_gc(curr_ir);
j++,i++){
_obj = callinfo_sfreg(curr_ci, j);
*_obj = *a2_getcstack(vm_p->env_p, i);
}
for(; j<ir_ga(curr_ir)+ir_gc(curr_ir); j++){
_obj = callinfo_sfreg(curr_ci, j);
obj_setX(_obj, A2_TNIL, point, NULL);
}
a2_setbottom(vm_p->env_p, _b);
a2_settop(vm_p->env_p, _top);
curr_pc++;
}
