int scan_mod()
/*
ptr mod
| "(" mod ")"
| mod suf
| ...
*/
{
for(;;)
switch (lookahead()) {
case AND:
case MUL: // ptr mod
continue;
case LP: // "(" mod ")" [suf] | suf
switch (lookahead()) {
case AND:
case MUL:
case LP:
case LB:
backup();
if (!scan_mod()) return 0;
if (lookahead() != RP) return 0;
if (!scan_suf()) return 0;
return 1;
case AGGR: case ENUM: case TYPE: case TNAME:
backup();
if (!scan_tlist()) return 0;
if (lookahead() != RP) return 0;
/* no break */
case RP:
bad_cast = 1; /* possible cast to ftn */
if (!scan_suf()) return 0;
return 1;
default:
return 0;
}
case LB: // mod suf
backup();
if (!scan_suf()) return 0;
return 1;
case RP:
case CM:
case ELLIPSIS:
backup();
return 1;
default:
return 0;
}
}
void scan_e()
/* scan expr in vec */
{
long brcount = 1L;
int localcast = 0;
for(;;)
switch (lookahead()) {
case RB:
if (--brcount == 0L) return;
continue;
case LB:
brcount++;
continue;
case LP:
if (localcast)
continue;
toknode* mark = latok;
if (scan_type())
if (scan_mod())
if (lookahead() == RP)
switch (lookahead()) {
case CM:
case RP:
case SM:
case LC:
case ASSIGN:
break;
default:
backup();
mark->tok = CAST;
latok->tok = ENDCAST;
}
continue;
case CAST: /* scenario: local cast recognized,
main cast in lalex fails,
lalex later recognizes a smaller cast
containing this one
*/
localcast++;
continue;
case ENDCAST:
localcast--;
continue;
default:
continue;
}
}
int scan_tlist()
/*
tlist type | type
type --> (TYPE | [AGGR] TNAME) mod
*/
{
for(;;) {
switch (lookahead()) {
case AGGR:
case ENUM:
get_tag();
case TYPE:
case TNAME:
scan_type();
break;
case ELLIPSIS:
if (lookahead() != RP) {
error("missing ')' after '...'");
insert_tok(RP);
}
case RP:
backup();
return 1;
default:
return 0;
}
/* saw type */
if (!scan_mod()) return 0;
switch (lookahead()) {
case CM:
continue;
case ELLIPSIS:
if (lookahead() != RP) {
error("missing ')' after '...'");
insert_tok(RP);
}
case RP:
backup();
return 1;
default:
return 0;
}
}
}
TOK lalex()
{
static nocast = 0; // prevent ID CAST, FOR CAST, etc.
//static in_op = 0; // prevent OPERATOR op CAST
static incast = 0; // don't scan in already recognized cast
static in_enum;
static fr;
char en = 0;
switch (la_start()) {
case VOLATILE:
case SIGNED:
error('w',"keyword%k (ignored)",latok->tok);
return lalex();
case ENUM:
en = 1;
case AGGR:
switch (tk) {
/* strictly speaking these ought to be included,
but they only cause grief
case ELSE:
case COLON: // label
case RP: // end of condition
*/
case 0:
case LP: // argument list
case CM:
case NEW:
case CAST:
case RP: // yok, old C: f(i) struct s * i; {}
case OPERATOR:
case DO:
case TYPE: // might be "const"
case COLON: // label
case SM: // end of statement
case RC: // end of statement
case LC: // first statement
break;
default:
error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok);
return tk=SM;
}
{ TOK t = lookahead();
TOK x;
switch (t) {
case TNAME:
x = lookahead();
break;
case ID: // hidden or undefined
x = lookahead();
backup();
switch (x) {
case LC:
in_enum = en;
case COLON: // defining: return AGGR ID
backup();
fr = 0;
DO_RET;
default:
{ Pname n = ktbl->look(latok->retval.s,HIDDEN);
if (n == 0) { // new tag: define it
n = new name(latok->retval.s);
n->lex_level = 0;
n = n->tname(latok->last->retval.t);
modified_tn = modified_tn->l;
}
else {
switch (n->tp->base) {
case COBJ:
case EOBJ:
break;
default:
error('i',"hidden%n:%t",n,n->tp);
}
}
latok->tok = TNAME;
latok->retval.pn = n;
}
}
(void)lookahead();
break;
case LC:
in_enum = en;
default:
fr = 0;
DO_RET;
};
switch (x) {
case LC: // class x {
in_enum = en;
case COLON: // class x :
fr = 0;
DO_RET;
case SM:
if (tk!=NEW && fr==0) { // no further processing necessary
deltok(); // class
deltok(); // x
deltok(); // ;
return lalex();
}
// new class x ; => new x ;
default:
deltok(); // AGGR(?) TNAME(x) => TNAME(x)
fr = 0;
DO_RET;
}
}
case LP:
fr = 0;
if (nocast) {
nocast = 0;
DO_RET;
// } else if (in_op) {
// in_op = 0;
// DO_RET;
} else if (incast)
DO_RET;
/* possible cast */
bad_cast = 0;
if (scan_type()) {
if (scan_mod()) {
if (lookahead() != RP) DO_RET;
switch (lookahead()) {
case CM: case RP: case SM: case LC: case ASSIGN:
/* arg type list in declaration */
if (tk != SIZEOF) DO_RET;
break;
case PLUS: case MINUS: case MUL: case AND:
case NEW: case DELETE: case SIZEOF: case MEM:
case NOT: case COMPL: case ICOP:
case LP: case CAST:
case ID: case TYPE: case TNAME:
case THIS: case OPERATOR: case ZERO:
case ICON: case FCON: case CCON: case STRING:
// cast of a term
break;
default: // something wrong...
// binary op, missing ;, etc.
// "bad cast" could be legal expr
// "( TNAME() )" (ctor call)
if (bad_cast) DO_RET;
else break;
}
backup();
front->tok = CAST;
latok->tok = ENDCAST;
if (bad_cast) {
error("can't cast to function");
rep_cast();
}
incast = 1;
}
}
DO_RET;
case CAST:
incast++;
DO_RET;
case ENDCAST:
if (--incast == 0) nocast = 0;
DO_RET;
case ID:
{ char* s = front->retval.s;
fr = 0;
nocast = 1;
switch (lookahead()) {
case ID:
{ // handle ID ID
// assume ID is a missing, hidden, or misspelt TNAME
char* s2 = latok->retval.s;
backup();
Pname n = ktbl->look(s,HIDDEN);
if (n == 0) { // new tag: define it
error("%s %s:TX (%s is not a TN)",s,s2,s);
n = new name(s);
n->lex_level = 0;
n = n->tname(0);
modified_tn = modified_tn->l;
n->tp = any_type;
}
else {
error("%s %s: %s is hidden",s,s2,s);
}
latok->tok = TNAME;
latok->retval.pn = n;
break;
}
case LC:
backup();
front->retval.pn = new name(s);
front->retval.pn->lex_level--;
break;
default:
backup();
front->retval.pn = new name(s);
}
DO_RET;
}
case CASE:
case DEFAULT:
case PUBLIC:
case ELSE:
fr = 0;
switch (tk) {
case COLON: // label
case SM: // end of statement
case RC: // end of statement
case LC: // first statement
DO_RET;
default:
error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok);
return tk=SM;
}
case DO:
case GOTO:
case CONTINUE:
case BREAK:
case RETURN:
fr = 0;
switch (tk) {
case ELSE:
case DO:
case COLON: // label
case RP: // end of condition
case SM: // end of statement
case RC: // end of statement
case LC: // first statement
DO_RET;
default:
error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok);
return tk=SM;
}
case IF:
case WHILE:
case FOR:
case SWITCH:
fr = 0;
switch (tk) {
case ELSE:
case DO:
case COLON: // label
case RP: // end of condition
case SM: // end of statement
case RC: // end of statement
case LC: // first statement
nocast = 1;
DO_RET;
default:
error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok);
return tk=SM;
}
case TYPE: // dangerous to diddle with: constructor notation
fr = 0;
switch (tk) {
case ID:
// case RP: old C function definition
case RB:
error(&curloc,"';' missing afterS orD before\"%k\"",latok->tok);
return tk=SM;
}
if (latok->retval.t == FRIEND) fr = 1;
nocast = 1;
DO_RET;
case TNAME: // dangerous to diddle with: name hiding
{ Pname n = latok->retval.pn;
if (fr) { // guard against: TYPE(friend) TNAME(x) SM
nocast = 1;
fr = 0;
DO_RET;
}
fr = 0;
// fprintf(stderr,"type or tname %d %s\n",tk,n->string); fflush(stderr);
switch (tk) {
case TYPE: // int TN ? or unsigned TN ?
// beware of unsigned etc.
switch (lookahead()) {
case SM:
case RB:
case COLON:
case ASSIGN:
goto hid;
// case LP: // the real problem
default:
nocast = 1;
DO_RET;
}
case TNAME: // TN TN ?
switch (lookahead()) {
case MEM: // cl_name::mem_name
case DOT: // anachronism: cl_name.mem_name
nocast = 1;
DO_RET;
}
hid:
backup(); // undo lookahead after TNAME
n->hide();
n = new name(n->string);
n->n_oper = TNAME;
latok->tok = ID;
latok->retval.pn = n;
}
}
case NEW:
fr = 0;
nocast = 1;
DO_RET;
/*
case OPERATOR:
switch (lookahead()) {
case LP:
in_op = 1;
if (lookahead() != RP) error("bad operator");
break;
case LB:
if (lookahead() != RB) error("bad operator");
break;
case TYPE:
case TNAME:
while (lookahead() == MUL) ;
backup();
// default : 'regular' operator
}
if (lookahead() == LP) in_op = 1;
DO_RET;
*/
case RC:
fr = 0;
switch (tk) {
case RC: // nested } (either stmt or expr)
case LC: // empty block: {}
case SM: // end of statement
break;
default:
{ TOK t;
loc x = curloc;
switch (t = lookahead()) {
case ELSE:
case RC: // } } probably end of initializer
case CM: // } , definitely end of initializer
case SM: // } ; probably end of initializer or class
case RP: // int f( struct { ... } );
break;
default:
// either "= { ... E } SorD"
// or " SorD }"
// or enum { a, b } c; - yuk
if (in_enum == 0) {
error(&x,"';'X at end ofS orD before '}'");
return tk=SM;
}
in_enum = 0;
}
}
}
in_enum = 0;
default:
fr = 0;
nocast = 0;
DO_RET;
}
ret:
// hand optimized return:
//TOK deltok()
//{
toknode* T = front;
tk = T->tok;
yylval = T->retval;
if (front = front->next) front->last = 0;
delete T;
return tk;
//}
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
std::cout << "got a message!!!" <<std::endl;
if (listener_ == 0 || projector_ == 0) {
return;
}
std::cout << "GOT A SCAN!!!" << std::endl;
sensor_msgs::LaserScan scan_mod(*scan_in);
scan_mod.time_increment = (float)1.0/(360*5);
scan_mod.scan_time = (float)1.0/5;
if(!listener_->waitForTransform(
scan_in->header.frame_id,
"/odom",
scan_in->header.stamp + ros::Duration().fromSec(scan_mod.ranges.size()*scan_mod.time_increment),
ros::Duration(1.0))){
return;
}
double t_start = ros::Time::now().toSec();
int best_matching_offset = -1;
float best_match_val = -1.0;
for (int t_offset_msecs = 200; t_offset_msecs <= 400; t_offset_msecs += 10) {
float match_val = 0.0;
sensor_msgs::PointCloud cloud;
scan_mod.header.stamp = scan_in->header.stamp - ros::Duration(t_offset_msecs/1000.0);
try {
projector_->transformLaserScanToPointCloud("/odom",scan_mod,
cloud,*listener_);
if (prev_cloud == 0) {
prev_cloud = new sensor_msgs::PointCloud(cloud);
return;
}
for (int i = 0; i < cloud.points.size(); i++) {
float min_val = -1.0;
float dist;
for (int j = 0; j < prev_cloud->points.size(); j++){
dist = sqrt((prev_cloud->points[j].x - cloud.points[i].x)*(prev_cloud->points[j].x - cloud.points[i].x) +
(prev_cloud->points[j].y - cloud.points[i].y)*(prev_cloud->points[j].y - cloud.points[i].y) +
(prev_cloud->points[j].z - cloud.points[i].z)*(prev_cloud->points[j].z - cloud.points[i].z));
if (min_val == -1 || dist < min_val) {
min_val = dist;
}
}
match_val += min_val;
}
if (best_matching_offset == -1 || match_val < best_match_val) {
best_match_val = match_val;
best_matching_offset = t_offset_msecs;
}
} catch (...) {
}
}
// set this based on the best matching offset
if (best_matching_offset < 0) {
best_matching_offset = 0;
}
scan_mod.header.stamp = scan_in->header.stamp - ros::Duration(best_matching_offset/1000.0);
std::cout << "PROCESSED A SCAN!!! " << best_matching_offset << " " << ros::Time::now().toSec() - t_start << std::endl;
// Do something with cloud.
pub.publish(scan_mod);
try {
sensor_msgs::PointCloud final_cloud;
projector_->transformLaserScanToPointCloud("/odom",scan_mod,
final_cloud,*listener_);
delete prev_cloud;
prev_cloud = new sensor_msgs::PointCloud(final_cloud);
} catch (...) {}
}
