static void
scrolling_plots_draw (Viewer *viewer, Renderer *renderer)
{
RendererScrollingPlots *self = (RendererScrollingPlots*) renderer->user;
if (!self->max_utime) return;
GLdouble model_matrix[16];
GLdouble proj_matrix[16];
GLint viewport[4];
glGetDoublev (GL_MODELVIEW_MATRIX, model_matrix);
glGetDoublev (GL_PROJECTION_MATRIX, proj_matrix);
glGetIntegerv (GL_VIEWPORT, viewport);
double gs_ts_max = self->max_utime * 1e-6;
double gs_ts_min = gs_ts_max -
bot_gtk_param_widget_get_double (self->pw, PARAM_NAME_GRAPH_TIMESPAN);
bot_gl_scrollplot2d_set_xlim (self->psi_distance_plot, gs_ts_min, gs_ts_max);
int plot_width = bot_gtk_param_widget_get_int (self->pw, PARAM_NAME_SIZE);
int plot_height = plot_width / 2;
int x = viewport[2] - plot_width - 10;
int y = viewport[1] +10 ;
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_RENDER_PSI_DISTANCE)) {
bot_gl_scrollplot2d_gl_render_at_window_pos (self->psi_distance_plot,
x, y, plot_width, plot_height);
y += plot_height;
}
}
static void
scrolling_plots_draw (Viewer *viewer, Renderer *renderer)
{
RendererScrollingPlots *self = (RendererScrollingPlots*) renderer->user;
if (!self->max_utime) return;
GLdouble model_matrix[16];
GLdouble proj_matrix[16];
GLint viewport[4];
glGetDoublev (GL_MODELVIEW_MATRIX, model_matrix);
glGetDoublev (GL_PROJECTION_MATRIX, proj_matrix);
glGetIntegerv (GL_VIEWPORT, viewport);
double gs_ts_max = self->max_utime * 1e-6;
double gs_ts_min = gs_ts_max -
bot_gtk_param_widget_get_double (self->pw, PARAM_NAME_GRAPH_TIMESPAN);
bot_gl_scrollplot2d_set_xlim (self->navigator_rotation_plot, gs_ts_min, gs_ts_max);
bot_gl_scrollplot2d_set_xlim (self->navigator_translation_plot, gs_ts_min, gs_ts_max);
bot_gl_scrollplot2d_set_xlim (self->encoder_left_plot, gs_ts_min, gs_ts_max);
bot_gl_scrollplot2d_set_xlim (self->encoder_right_plot, gs_ts_min, gs_ts_max);
int plot_width = bot_gtk_param_widget_get_int (self->pw, PARAM_NAME_SIZE);
int plot_height = plot_width / 3;
int x = viewport[2] - plot_width;
int y = viewport[1];
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_RENDER_NAVIGATOR_ROTATION)) {
bot_gl_scrollplot2d_gl_render_at_window_pos (self->navigator_rotation_plot,
x, y, plot_width, plot_height);
y += plot_height;
}
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_RENDER_NAVIGATOR_TRANSLATION)) {
bot_gl_scrollplot2d_gl_render_at_window_pos (self->navigator_translation_plot,
x, y, plot_width, plot_height);
y += plot_height;
}
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_RENDER_ENCODER_LEFT)) {
bot_gl_scrollplot2d_gl_render_at_window_pos (self->encoder_left_plot,
x, y, plot_width, plot_height);
y += plot_height;
}
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_RENDER_ENCODER_RIGHT)) {
bot_gl_scrollplot2d_gl_render_at_window_pos (self->encoder_right_plot,
x, y, plot_width, plot_height);
y += plot_height;
}
}
static void on_param_widget_changed(BotGtkParamWidget *pw, const char *name, void *user)
{
RendererLaser *self = (RendererLaser*) user;
if (!self->viewer)
return;
self->param_scan_memory = bot_gtk_param_widget_get_int(self->pw, PARAM_SCAN_MEMORY);
self->param_color_mode = bot_gtk_param_widget_get_enum(self->pw, PARAM_COLOR_MODE);
self->param_z_buffer = bot_gtk_param_widget_get_bool(self->pw, PARAM_Z_BUFFER);
self->param_big_points = bot_gtk_param_widget_get_bool(self->pw, PARAM_BIG_POINTS);
self->param_spacial_decimate = bot_gtk_param_widget_get_bool(self->pw, PARAM_SPATIAL_DECIMATE);
self->z_relative = bot_gtk_param_widget_get_bool(self->pw, PARAM_Z_RELATIVE);
self->param_color_mode_z_max_z = bot_gtk_param_widget_get_double(self->pw, PARAM_COLOR_MODE_Z_MAX_Z);
self->param_color_mode_z_min_z = bot_gtk_param_widget_get_double(self->pw, PARAM_COLOR_MODE_Z_MIN_Z);
self->param_max_draw_z = bot_gtk_param_widget_get_double(self->pw, PARAM_MAX_DRAW_Z);
self->param_min_draw_z = bot_gtk_param_widget_get_double(self->pw, PARAM_MIN_DRAW_Z);
self->param_max_draw_range = bot_gtk_param_widget_get_double(self->pw, PARAM_MAX_DRAW_RANGE);
self->param_alpha = bot_gtk_param_widget_get_double(self->pw, PARAM_ALPHA);
self->param_max_buffer_size = bot_gtk_param_widget_get_int(self->pw, PARAM_MAX_BUFFER_SIZE);
for (int i = 0; i < self->channels->len; i++) {
laser_channel *lchan = g_ptr_array_index(self->channels, i);
lchan->enabled = bot_gtk_param_widget_get_bool(pw, lchan->name);
}
bot_viewer_request_redraw(self->viewer);
}
static void
_renderer_draw (BotViewer *viewer, BotRenderer *super)
{
GfeRenderer *self = (GfeRenderer*) super->user;
glEnable(GL_DEPTH_TEST);
if (!self->display_lists_ready) {
if (self->wavefront_model)
self->wavefront_dl = compile_display_list (self, "wavefront", self->wavefront_model);
self->wavefront_dl2 = compile_display_list (self, "wavefront", self->wavefront_model2); //edited
self->display_lists_ready = 1;
}
glPushMatrix();
// These are all widgets. Attempt to remove these widget parameters and input actual rotational values
double scale = bot_gtk_param_widget_get_double(self->pw, PARAM_SCALE);
glScalef (scale, scale, scale);
double rot_x = bot_gtk_param_widget_get_double(self->pw, PARAM_ROT_X);
double rot_y = bot_gtk_param_widget_get_double(self->pw, PARAM_ROT_Y);
double rot_z = bot_gtk_param_widget_get_double(self->pw, PARAM_ROT_Z);
glRotatef(rot_z, 0, 0, 1);
glRotatef(rot_y, 0, 1, 0);
glRotatef(rot_x, 1, 0, 0);
double dx = bot_gtk_param_widget_get_double(self->pw, PARAM_DX);
double dy = bot_gtk_param_widget_get_double(self->pw, PARAM_DY);
double dz = bot_gtk_param_widget_get_double(self->pw, PARAM_DZ);
glTranslated(dx, dy, dz);
if (self->display_lists_ready && self->wavefront_dl)
draw_wavefront_model (self);
glPopMatrix();
}
static void on_param_widget_changed(BotGtkParamWidget *pw, const char *name, void *user)
{
RendererFrames *self = (RendererFrames*) user;
if (self->updating) {
return;
}
BotViewer *viewer = self->viewer;
int activeSensorNum = bot_gtk_param_widget_get_enum(pw, PARAM_FRAME_SELECT);
if (!strcmp(name, PARAM_FRAME_SELECT)) {
if (activeSensorNum > 0) {
self->updating = 1;
bot_viewer_set_status_bar_message(self->viewer, "Modify Calibration relative to %s", bot_frames_get_relative_to(
self->frames, self->frameNames[activeSensorNum]));
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 1);
frames_update_handler(self->frames, self->frameNames[activeSensorNum], bot_frames_get_relative_to(self->frames,
self->frameNames[activeSensorNum]), bot_timestamp_now(), self);
}
else {
bot_viewer_set_status_bar_message(self->viewer, "");
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 0);
}
}
else if (!strcmp(name, PARAM_SAVE) && activeSensorNum > 0) {
char save_fname[1024];
sprintf(save_fname, "manual_calib_%s.cfg", self->frameNames[activeSensorNum]);
fprintf(stderr, "saving params to: %s\n", save_fname);
FILE * f = fopen(save_fname, "w");
double pos[3];
pos[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
pos[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
pos[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL);
rpy[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH);
rpy[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_YAW);
double rpy_rad[3];
for (int i = 0; i < 3; i++)
rpy_rad[i] = bot_to_radians(rpy[i]);
double quat[4];
bot_roll_pitch_yaw_to_quat(rpy_rad, quat);
double rod[3];
bot_quat_to_rodrigues(quat, rod);
fprintf(f, ""
"%s {\n"
"position = [%f, %f, %f];\n"
"rpy = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rpy[0], rpy[1], rpy[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fprintf(f, ""
"\n"
"%s {\n"
"position = [%f, %f, %f];\n"
"rodrigues = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rod[0], rod[1], rod[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fclose(f);
bot_viewer_set_status_bar_message(self->viewer, "Calibration saved to %s", save_fname);
}
else if (activeSensorNum > 0) {
self->updating = 1;
BotTrans curr;
curr.trans_vec[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
curr.trans_vec[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
curr.trans_vec[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL));
rpy[1] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH));
rpy[2] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_YAW));
bot_roll_pitch_yaw_to_quat(rpy, curr.rot_quat);
if (fabs(rpy[0]) > M_PI || fabs(rpy[1]) > M_PI || fabs(rpy[2]) > M_PI) {
bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(bot_mod2pi(rpy[0])));
bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(bot_mod2pi(rpy[1])));
bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(bot_mod2pi(rpy[2])));
}
//and update the link
const char * frame_name = self->frameNames[activeSensorNum];
const char * relative_to = bot_frames_get_relative_to(self->frames, frame_name);
bot_frames_update_frame(self->frames, frame_name, relative_to, &curr, bot_timestamp_now());
}
bot_viewer_request_redraw(self->viewer);
}
