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); }