void draw_robot(struct Vertex position, double heading)
{
translate_robot(position);
rotate_robot(heading);
draw_bumper();
draw_wheels();
draw_body();
}
void interactive(machine *m) {
/* Set up the ncurses interface */
ui_info ui;
initscr();
start_color();
raw();
noecho();
keypad(stdscr, true);
/* Figure out the level of color support, choose attributes accordingly */
if (has_colors()) {
if (can_change_color()) {
/* Best case, programmable colors */
init_color(CLR_WHITEGRAY, 800, 800, 800); /* dull text */
init_color(CLR_DARKGRAY, 400, 400, 400); /* highlighted metal */
init_color(CLR_DARKESTGRAY, 250, 250, 250); /* darkest metal */
init_color(CLR_BRIGHTRED, 1000, 400, 400); /* coded text */
init_pair(CP_WHEEL_PLAIN, CLR_WHITEGRAY, CLR_DARKESTGRAY);
init_pair(CP_WHEEL_ACTIV, COLOR_WHITE, CLR_DARKGRAY);
init_pair(CP_PLAIN, COLOR_WHITE, COLOR_BLACK);
init_pair(CP_CODED, CLR_BRIGHTRED, COLOR_BLACK);
init_pair(CP_BTN, COLOR_RED, CLR_DARKESTGRAY);
init_pair(CP_BTNH, COLOR_RED, CLR_DARKGRAY);
} else {
/* Second best, 8 color ncurses */
init_pair(CP_WHEEL_PLAIN, COLOR_YELLOW, COLOR_BLUE);
init_pair(CP_WHEEL_ACTIV, COLOR_WHITE, COLOR_RED);
init_pair(CP_PLAIN, COLOR_WHITE, COLOR_BLACK);
init_pair(CP_CODED, COLOR_RED, COLOR_BLACK);
init_pair(CP_BTN, COLOR_RED, COLOR_BLUE);
init_pair(CP_BTNH, COLOR_BLUE, COLOR_RED);
}
ui.attr_plain = COLOR_PAIR(CP_PLAIN);
ui.attr_coded = COLOR_PAIR(CP_CODED);
ui.attr_wheel_plain = COLOR_PAIR(CP_WHEEL_PLAIN);
ui.attr_wheel_activ = COLOR_PAIR(CP_WHEEL_ACTIV) | A_BOLD;
ui.attr_btn = COLOR_PAIR(CP_BTN);
ui.attr_btnh = COLOR_PAIR(CP_BTNH);
} else {
/* No color fallback */
ui.attr_plain = A_NORMAL;
ui.attr_coded = A_BOLD;
ui.attr_wheel_plain = A_REVERSE;
ui.attr_wheel_activ = A_REVERSE | A_BOLD | A_UNDERLINE;
ui.attr_btn = A_REVERSE | A_BOLD;
ui.attr_btnh = ui.attr_btn;
}
ui.attr_lbl = A_NORMAL;
ui.attr_lblh = A_BOLD;
ui.chosen_wheel = -1;
/* Make the windows */
int topheight = 10 + m->wheelslots;
int longestname = strlen("ring settings");
if (longestname < m->longest_wheelname) longestname = m->longest_wheelname;
int topwidth = 4 * m->wheelslots + 1 + longestname;
int namelen = wcslen(m->name);
if (namelen > topwidth) topwidth = namelen;
ui.w_wheels = newwin(topheight, topwidth, 0, COLS-topwidth);
int botheight = ((LINES - topheight) / 3) * 3 - 1;
int botwidth = COLS;
ui.w_code = newwin(botheight, botwidth, topheight, 0);
ui.w_pop = newwin(botheight, botwidth, topheight, 0);
draw_wheels(m, &ui);
bool enciphering = true; /* enchiper or dechiper */
wchar_t plaintext[MAXLINE+1]; /* typed/dechipered text */
wchar_t ciphertxt[MAXLINE+1]; /* typed/enchipered text */
memset(plaintext, 0, sizeof(wchar_t)*(MAXLINE+1));
memset(ciphertxt, 0, sizeof(wchar_t)*(MAXLINE+1));
int textpos = 0;
int maxpos = COLS - 2 > MAXLINE ? MAXLINE : COLS - 2;
curses_bug_workaround();
wmove(ui.w_code, 1, 1);
/* main loop. The first event has to be the KEY_RESIZE, it creates the display! */
int rc = KEY_CODE_YES;
wint_t wch = KEY_RESIZE;
for (bool active = true; active; rc = active ? get_wch(&wch) : 0) {
switch (rc) {
case KEY_CODE_YES: /* specials */
switch (wch) {
case KEY_F(1):
highlight_wheel(m, &ui, 0);
break;
case KEY_F(2):
highlight_wheel(m, &ui, 1);
break;
case KEY_F(3):
highlight_wheel(m, &ui, 2);
break;
case KEY_F(4):
highlight_wheel(m, &ui, 3);
break;
case KEY_F(5):
highlight_wheel(m, &ui, 4);
break;
case KEY_F(6):
highlight_wheel(m, &ui, 5);
break;
case KEY_F(7):
highlight_wheel(m, &ui, 6);
break;
case KEY_F(8):
highlight_wheel(m, &ui, 7);
break;
case KEY_F(9):
highlight_wheel(m, &ui, 8);
break;
case KEY_LEFT:
highlight_left(m, &ui);
break;
case KEY_RIGHT:
highlight_right(m, &ui);
break;
case KEY_F(10):
highlight_wheel(m, &ui, 9);
break;
case KEY_NPAGE:
next_wheel(m, &ui);
break;
case KEY_UP:
if (ui.chosen_wheel == -1) {
/* switch to encoding */
enciphering = true;
wmove(ui.w_code, 1, textpos+1);
wnoutrefresh(ui.w_code);
} else wheel_turn(m, &ui, -1);
break;
case KEY_DOWN:
if (ui.chosen_wheel == -1) {
/* switch to decoding */
enciphering = false;
wmove(ui.w_code, 2, textpos+1);
wnoutrefresh(ui.w_code);
} else wheel_turn(m, &ui, 1);
break;
case KEY_RESIZE: /* user resized the xterm - redraw all! */
curses_bug_workaround();//Remove, and top window blanks out
/* Must repaint all, as downsizing may blank the terminal */
/* Deal with the code wheel window */
botheight = ((LINES - topheight) / 3) * 3 - 1;
botwidth = COLS;
wresize(ui.w_code, botheight, botwidth);
wresize(ui.w_pop, botheight, botwidth);
int oldx = getbegx(ui.w_wheels);
int newx = COLS-topwidth;
mvwin(ui.w_wheels, 0, COLS-topwidth); /* move the window */
/* now clear out the exposed screen area */
if (newx > oldx) {
int xlen = newx-oldx;
char *spc = malloc(xlen+1);
memset(spc, ' ', xlen);
spc[xlen] = 0;
for (int i=0; i < topheight; ++i) mvprintw(i, oldx, spc);
free(spc);
}
wnoutrefresh(ui.w_wheels);
curses_bug_workaround(); //Remove, and the cursor will misplaced when upsizing
/* Now the code text window */
maxpos = COLS - 2 > MAXLINE ? MAXLINE : COLS - 2;
if (textpos > maxpos) {
leftcut(plaintext, textpos - maxpos, maxpos+1);
leftcut(ciphertxt, textpos - maxpos, maxpos+1);
textpos = maxpos;
wattrset(ui.w_code, ui.attr_plain);
mvwprintw(ui.w_code, 1, 1, "%ls", plaintext);wclrtoeol(ui.w_code);
wattrset(ui.w_code, ui.attr_coded);
mvwprintw(ui.w_code, 2, 1, "%ls", ciphertxt);wclrtoeol(ui.w_code);
if (enciphering) wmove(ui.w_code, 1, textpos+1);
}
wnoutrefresh(ui.w_code);
break;
}
break;
case OK:
if (iscntrl(wch)) switch (wch) {
/* ctrl tv change ring settings */
case 20:
ringstellung(m, &ui, -1);
break;
case 22:
ringstellung(m, &ui, 1);
break;
/* quit on ctrl+c */
case 3:
active = false;
break;
/* unselect wheel on enter */
case '\n':
highlight_wheel(m, &ui, -1);
wnoutrefresh(ui.w_code);
break;
}
/* plain typing */
else {
if (ui.chosen_wheel > -1) highlight_wheel(m, &ui, -1);
/* Need a line break first? */
if (textpos >= maxpos) {
/* add scrolling later !!! for now, just clear */
textpos = 0;
memset(plaintext, 0, sizeof(wchar_t)*(MAXLINE+1));
memset(ciphertxt, 0, sizeof(wchar_t)*(MAXLINE+1));
if (enciphering) {
wmove(ui.w_code, 2, textpos+1); wclrtoeol(ui.w_code);
wmove(ui.w_code, 1, textpos+1); wclrtoeol(ui.w_code);
} else {
wmove(ui.w_code, 1, textpos+1); wclrtoeol(ui.w_code);
wmove(ui.w_code, 2, textpos+1); wclrtoeol(ui.w_code);
}
}
if (enciphering) {
plaintext[textpos] = wch;
ciphertxt[textpos] = encipher(m, wch, &ui);
wattrset(ui.w_code, ui.attr_coded);
mvwprintw(ui.w_code, 2, 1, "%ls", ciphertxt);
wattrset(ui.w_code, ui.attr_plain);
mvwprintw(ui.w_code, 1, 1, "%ls", plaintext);
} else {
ciphertxt[textpos] = wch;
plaintext[textpos] = decipher(m, wch, &ui);
wattrset(ui.w_code, ui.attr_plain);
mvwprintw(ui.w_code, 1, 1, "%ls", plaintext);
wattrset(ui.w_code, ui.attr_coded);
mvwprintw(ui.w_code, 2, 1, "%ls", ciphertxt);
}
textpos++;
wnoutrefresh(ui.w_wheels);
wnoutrefresh(ui.w_code);
break;
}
}
doupdate();
}
endwin();
}
static void
car_draw (Viewer *viewer, Renderer *super)
{
RendererCar *self = (RendererCar*) super->user;
lcmtypes_pose_t pose;
if (ctrans_local_pose (self->ctrans, &pose) < 0)
return;
GtkuParamWidget *pw = self->pw;
int bling = self->chassis_model ?
gtku_param_widget_get_bool (pw, PARAM_NAME_BLING) : 0;
int wheels = self->wheel_model ?
gtku_param_widget_get_bool (pw, PARAM_NAME_WHEELS) : 0;
if ((bling || wheels) && !self->display_lists_ready) {
load_bling (self);
}
glColor4f(0,1,0,0.75);
glLineWidth (10);
glBegin(GL_LINE_STRIP);
glVertex3dv (self->last_pose.pos);
for (unsigned int i = 0;
i < MIN (gu_ptr_circular_size(self->path), self->max_draw_poses);
i++) {
glVertex3dv(gu_ptr_circular_index(self->path, i));
}
glEnd();
glPushMatrix();
// compute the rotation matrix to orient the vehicle in world
// coordinates
double body_quat_m[16];
rot_quat_pos_to_matrix(pose.orientation, pose.pos, body_quat_m);
// opengl expects column-major matrices
double body_quat_m_opengl[16];
matrix_transpose_4x4d (body_quat_m, body_quat_m_opengl);
// rotate and translate the vehicle
glMultMatrixd (body_quat_m_opengl);
glEnable (GL_DEPTH_TEST);
if (bling && self->display_lists_ready && self->chassis_dl)
draw_chassis_model (self);
else
draw_footprint (self);
if (wheels && self->display_lists_ready && self->wheel_dl)
draw_wheels (self);
glPopMatrix();
if (self->display_detail) {
char buf[256];
switch (self->display_detail)
{
case DETAIL_SPEED:
sprintf(buf, "%.2f m/s",
sqrt(sq(pose.vel[0]) + sq(pose.vel[1]) + sq(pose.vel[2])));
break;
case DETAIL_RPY:
{
double rpy[3];
rot_quat_to_roll_pitch_yaw(pose.orientation, rpy);
sprintf(buf, "r: %6.2f\np: %6.2f\ny: %6.2f", to_degrees(rpy[0]), to_degrees(rpy[1]), to_degrees(rpy[2]));
break;
}
case DETAIL_GPS:
{
double lle[3], q[4];
ctrans_gps_pose(self->ctrans, lle, q);
double rpy[3];
rot_quat_to_roll_pitch_yaw(q, rpy);
sprintf(buf, "%15.7f %15.7f\nelev: %10.3f\nhead: %6.2f\n", lle[0], lle[1], lle[2], to_degrees(rpy[2]));
break;
}
}
glColor3f(1,1,1);
glutil_draw_text(pose.pos, GLUT_BITMAP_HELVETICA_12, buf,
GLUTIL_DRAW_TEXT_DROP_SHADOW);
}
}
