void exitMarkerCallback(const visualization_msgs::Marker& marker) {
    ROS_INFO("received exit-point marker");
    cout<<"frame_id="<<marker.header.frame_id<<endl;
    ROS_INFO("coors w/rt frame_id: %f, %f, %f",marker.pose.position.x,marker.pose.position.y,marker.pose.position.z);
    tf::StampedTransform stfMarkerWrtCamera;
    //string marker_frame(marker.header.frame_id);
    g_tfListener_ptr->lookupTransform("left_camera_optical_frame", marker.header.frame_id, ros::Time(0), stfMarkerWrtCamera);
    tf::Transform tfMarkerWrtCamera(stfMarkerWrtCamera.getBasis(),stfMarkerWrtCamera.getOrigin()); //get tf from stamped tf
    geometry_msgs::Point marker_pt = marker.pose.position;
    geometry_msgs::Point marker_pt_wrt_camera = xform_point(marker_pt,tfMarkerWrtCamera);
    ROS_INFO("marker coords w/rt camera: %f, %f, %f",marker_pt_wrt_camera.x,marker_pt_wrt_camera.y,marker_pt_wrt_camera.z);
    g_O_exit_point(0) = marker_pt_wrt_camera.x;
    g_O_exit_point(1) = marker_pt_wrt_camera.y;
    g_O_exit_point(2) = marker_pt_wrt_camera.z;
    //g_got_new_points = true;
}
Пример #2
0
int add_inst(LEXER *lp, char *inst_name)
{
    enum {START,NUM1,NUM2,NUM3,NUM4,END} state = START;

    double x1, y1;	// pick value
    double x2, y2;	// pick value
    double x3, y3;	// pick value
    double xold=0.0, yold=0.0;	// previous pick value to suppress double pics
    int numpicks=0;	// number of picks
    int done=0;
    TOKEN token;
    OPTS opts;
    char *word;
    int debug=0;
    struct db_inst *ip;

    DB_TAB *ed_rep;

    XFORM *xp;

    double xx, yy;

    opt_set_defaults(&opts);

    // rl_saveprompt();
    rl_setprompt("ADD_INST> ");

    if (debug) printf("currep = %s\n", currep->name);
    if (debug) printf("adding inst %s\n", inst_name);

    /* don't destroy it if it's already in memory */
    if (debug) printf("calling db_lookup with %s\n", inst_name);

    if (loadrep(inst_name) == 0) {
	printf("warning: you can't add a null instance: %s\n", inst_name);
	token_flush_EOL(lp);
	done++;	
    }

    if ((ed_rep = db_lookup(inst_name)) == 0) {
    	printf("ADD INST: instance not found: %s\n", inst_name );
	return(-1);
    }

    bb_xmin=ed_rep->minx;
    bb_xmax=ed_rep->maxx;
    bb_ymin=ed_rep->miny;
    bb_ymax=ed_rep->maxy;

    if (debug) printf("currep = %s\n", currep->name);

    if (strcmp(currep->name, inst_name) == 0 || db_contains(inst_name, currep->name)) {
    	printf("ADD INST: Sorry, to do that would cause a recursive definition\n" );
	return(-1);
    }

    while (!done) {
	token = token_look(lp, &word);
	if (debug) printf("got %s: %s state: %d\n", tok2str(token), word, state);
	if (token==CMD) {
	    state=END;
	} 
	switch(state) {	
	    case START:		/* get option or first xy pair */
	        db_checkpoint(lp);
		rubber_set_callback(draw_inst_bb);
		if (token == OPT ) {
		    token_get(lp, &word); 
		    if (opt_parse(word, INST_OPTS, &opts) == -1) {
			state = END;
		    } else {
			/* an option may have scaled the bounding box */
			/* clear callback, recompute and then restart */

			rubber_clear_callback(draw_inst_bb);

			xp = matrix_from_opts(&opts);
			bb_xmin = bb_xmax = bb_ymin = bb_ymax = 0.0;

			xx = ed_rep->minx;
			yy = ed_rep->miny;
			xform_point(xp, &xx, &yy); 

			if (xx < bb_xmin) bb_xmin = xx;
			if (yy < bb_ymin) bb_ymin = yy;
			if (xx > bb_xmax) bb_xmax = xx;
			if (yy > bb_ymax) bb_ymax = yy;

			xx = ed_rep->maxx;
			yy = ed_rep->maxy;
			xform_point(xp, &xx, &yy); 

			if (xx < bb_xmin) bb_xmin = xx;
			if (yy < bb_ymin) bb_ymin = yy;
			if (xx > bb_xmax) bb_xmax = xx;
			if (yy > bb_ymax) bb_ymax = yy;

			xx = ed_rep->maxx;
			yy = ed_rep->miny;
			xform_point(xp, &xx, &yy); 

			if (xx < bb_xmin) bb_xmin = xx;
			if (yy < bb_ymin) bb_ymin = yy;
			if (xx > bb_xmax) bb_xmax = xx;
			if (yy > bb_ymax) bb_ymax = yy;

			xx = ed_rep->minx;
			yy = ed_rep->maxy;
			xform_point(xp, &xx, &yy); 

			if (xx < bb_xmin) bb_xmin = xx;
			if (yy < bb_ymin) bb_ymin = yy;
			if (xx > bb_xmax) bb_xmax = xx;
			if (yy > bb_ymax) bb_ymax = yy;

			free(xp);

			state = START;
			rubber_set_callback(draw_inst_bb);
		    	if (opts.stepflag) {
			    rl_setprompt("ARRAY ORIGIN> ");
			    rubber_clear_callback();
			}
		    }
		} else if (token == NUMBER) {
		    if (opts.stepflag) {
			state = NUM2;	
		    } else {
			state = NUM1;	
		    }
		} else if (token == EOL) {
		    token_get(lp, &word); 	/* just eat it up */
		    state = START;
		} else if (token == EOC  || token == CMD) {
		    state = END; 
		} else {
		    token_err("INST", lp, "expected OPT or NUMBER", token);
		    state = END; 
		}
		break;
	    case NUM1:		/* get pair of xy coordinates */
		if (token == NUMBER) {
		    if (getnum(lp, "INST", &x1, &y1)) {
			// supress double clicks
			if (debug) printf("%d %g %g %g %g\n", numpicks, x1, y1, xold, yold);
			if (numpicks==0 || ((xold != x1) || (yold != y1))) {  
			    db_add_inst(currep, ed_rep, opt_copy(&opts), x1, y1);
			    rubber_clear_callback();
			    need_redraw++;
			    rubber_set_callback(draw_inst_bb);
			}
		        numpicks++; xold=x1; yold=y1;
			state = START;
	            } else {
			state = END;
		    }
		} else if (token == EOL) {
		    token_get(lp, &word);
		} else if (token == EOC  || token == CMD) {
		    state = END; 
		} else {
		    token_err("INST", lp, "expected NUMBER", token);
		    state = END; 
		}
		break;
	    case NUM2:		/* get column end coordinate */
		rubber_clear_callback();
		if (token == NUMBER) {
		    if (getnum(lp, "INST", &x1, &y1)) {
    			rl_setprompt("COLUMN EXTENT> ");
			state = NUM3;
	            } else {
			state = END;
		    }
		} else if (token == EOL) {
		    token_get(lp, &word);
		} else if (token == EOC  || token == CMD) {
		    state = END; 
		} else {
		    token_err("INST", lp, "expected NUMBER", token);
		    state = END; 
		}
		break;
	    case NUM3:		/* get column end coordinate */
		if (token == NUMBER) {
		    if (getnum(lp, "INST", &x3, &y3)) {
    			rl_setprompt("ROW EXTENT> ");
			state = NUM4;
	            } else {
			state = END;
		    }
		} else if (token == EOL) {
		    token_get(lp, &word);
		} else if (token == EOC  || token == CMD) {
		    state = END; 
		} else {
		    token_err("INST", lp, "expected NUMBER", token);
		    state = END; 
		}
		break;
	    case NUM4:		/* get column end coordinate */
		if (token == NUMBER) {
		    if (getnum(lp, "INST", &x2, &y2)) {
		        if (debug) printf("%g %g %g %g %g %g\n", x1, y1, x2, y2, x3, y3);
			ip = db_add_inst(currep, ed_rep, opt_copy(&opts), x1, y1);
			ip->colx = x3;
			ip->coly = y3;
			ip->rowx = x2;
			ip->rowy = y2;
			rubber_clear_callback();
			need_redraw++;
			state = END;
	            } else {
			state = END;
		    }
		} else if (token == EOL) {
		    token_get(lp, &word);
		} else if (token == EOC  || token == CMD) {
		    state = END; 
		} else {
		    token_err("INST", lp, "expected NUMBER", token);
		    state = END; 
		}
		break;
	    case END:
	    default:
		if (token == EOC || token == CMD) {
		    ;
		} else {
		    token_flush_EOL(lp);
		}
		done++;
		break;
	}
    }
    rubber_clear_callback();
    rl_restoreprompt();
    return(1);
}