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
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
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
on_param_widget_changed (BotGtkParamWidget *pw, const char *name, void *user)
{
RendererCar *self = (RendererCar*) user;
Viewer *viewer = self->viewer;
viewer->view_handler->follow_mode = 0;
if (bot_gtk_param_widget_get_bool(pw, PARAM_FOLLOW_POS))
viewer->view_handler->follow_mode |= FOLLOW_POS;
if (bot_gtk_param_widget_get_bool(pw, PARAM_FOLLOW_YAW))
viewer->view_handler->follow_mode |= FOLLOW_YAW;
self->max_draw_poses = bot_gtk_param_widget_get_int(pw, PARAM_MAXPOSES);
viewer_request_redraw ( self->viewer);
}
Renderer *renderer_scrolling_plots_new (Viewer *viewer)
{
RendererScrollingPlots *self =
(RendererScrollingPlots*) calloc (1, sizeof (RendererScrollingPlots));
self->viewer = viewer;
self->renderer.draw = scrolling_plots_draw;
self->renderer.destroy = scrolling_plots_free;
self->renderer.name = (char*)"Scrolling Plots";
self->renderer.user = self;
self->renderer.enabled = 1;
self->renderer.widget = gtk_alignment_new (0, 0.5, 1.0, 0);
self->lcm = globals_get_lcm ();
self->pw = BOT_GTK_PARAM_WIDGET (bot_gtk_param_widget_new ());
gtk_container_add (GTK_CONTAINER (self->renderer.widget),
GTK_WIDGET(self->pw));
gtk_widget_show (GTK_WIDGET (self->pw));
bot_gtk_param_widget_add_int (self->pw, PARAM_NAME_SIZE,
BOT_GTK_PARAM_WIDGET_SLIDER, 50, 800, 10, 150);
bot_gtk_param_widget_add_double (self->pw, PARAM_NAME_GRAPH_TIMESPAN,
BOT_GTK_PARAM_WIDGET_SLIDER, 1, 20, 0.5, 5);
bot_gtk_param_widget_add_booleans (self->pw,
BOT_GTK_PARAM_WIDGET_TOGGLE_BUTTON, PARAM_NAME_FREEZE, 0, NULL);
bot_gtk_param_widget_add_booleans (self->pw, (BotGtkParamWidgetUIHint) 0,
PARAM_NAME_RENDER_PSI_DISTANCE, 1, NULL);
bot_gtk_param_widget_add_booleans (self->pw, (BotGtkParamWidgetUIHint)0,
PARAM_NAME_SHOW_LEGEND, 0, NULL);
g_signal_connect (G_OBJECT (self->pw), "changed",
G_CALLBACK (on_param_widget_changed), self);
// psi_distance plot
self->psi_distance_plot = bot_gl_scrollplot2d_new ();
bot_gl_scrollplot2d_set_title (self->psi_distance_plot, "Psi distance");
bot_gl_scrollplot2d_set_text_color (self->psi_distance_plot, 0.7, 0.7, 0.7, 1);
bot_gl_scrollplot2d_set_bgcolor (self->psi_distance_plot, 0.1, 0.1, 0.1, 0.7);
bot_gl_scrollplot2d_set_border_color (self->psi_distance_plot, 1, 1, 1, 0.7);
bot_gl_scrollplot2d_set_ylim (self->psi_distance_plot, 0.2, 1);
bot_gl_scrollplot2d_add_plot (self->psi_distance_plot, "control", 1000);
bot_gl_scrollplot2d_set_color (self->psi_distance_plot, "control", 0, 0, 1, 1);
bot_gl_scrollplot2d_add_plot (self->psi_distance_plot, "status", 1000);
bot_gl_scrollplot2d_set_color (self->psi_distance_plot, "status", 1, 0, 0, 1);
// legends?
BotGlScrollPlot2dLegendLocation legloc = BOT_GL_SCROLLPLOT2D_HIDDEN;
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_SHOW_LEGEND)) {
legloc = BOT_GL_SCROLLPLOT2D_TOP_RIGHT;
}
bot_gl_scrollplot2d_set_show_legend (self->psi_distance_plot, legloc);
// subscribe to LCM messages
navlcm_class_param_t_subscribe (self->lcm, "CLASS_STATE", on_class_state, self);
return &self->renderer;
}
static void
on_param_widget_changed (BotGtkParamWidget *pw, const char *name,
RendererScrollingPlots *self)
{
if (! strcmp (name, PARAM_NAME_SHOW_LEGEND)) {
BotGlScrollPlot2dLegendLocation legloc = BOT_GL_SCROLLPLOT2D_HIDDEN;
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_SHOW_LEGEND)) {
legloc = BOT_GL_SCROLLPLOT2D_TOP_RIGHT;
}
bot_gl_scrollplot2d_set_show_legend (self->psi_distance_plot, legloc);
}
viewer_request_redraw (self->viewer);
}
static gboolean
on_gl_area_expose (GtkWidget * widget, GdkEventExpose * event, void* user_data)
{
app_t *self = (app_t*) user_data;
if (bot_gtk_param_widget_get_bool (self->param_widget, "Draw X")) {
glColor3f (0, 1, 0);
glBegin (GL_LINES);
glVertex2f (0, 0);
glVertex2f (self->img_width, self->img_height);
glVertex2f (self->img_width, 0);
glVertex2f (0, self->img_height);
glEnd ();
}
return FALSE;
}
static void on_param_widget_changed (BotGtkParamWidget *pw, const char *name, void *user)
{
BotLcmglRenderer *self = (BotLcmglRenderer*) user;
// iterate over each channel
GList *keys = bot_g_hash_table_get_keys(self->channels);
for (GList *kiter=keys; kiter; kiter=kiter->next) {
lcmgl_channel_t *chan = g_hash_table_lookup(self->channels,
kiter->data);
chan->enabled = bot_gtk_param_widget_get_bool (pw, kiter->data);
}
g_list_free (keys);
bot_viewer_request_redraw(self->viewer);
}
static void
on_class_state (const lcm_recv_buf_t *buf, const char *channel, const navlcm_class_param_t *msg, void *user)
{
RendererScrollingPlots *self = (RendererScrollingPlots *) user;
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_FREEZE))
return;
update_xaxis(self, msg->utime);
double timestamp = msg->utime * 1e-6;
bot_gl_scrollplot2d_add_point (self->psi_distance_plot, "status", timestamp,
msg->psi_distance);
bot_gl_scrollplot2d_add_point (self->psi_distance_plot, "control", timestamp,
msg->psi_distance_thresh);
viewer_request_redraw (self->viewer);
}
static void
on_motion_cmd (const lcm_recv_buf_t *buf, const char *channel, const fblcm_motion_cmd_t *msg, void *user)
{
RendererScrollingPlots *self = (RendererScrollingPlots *) user;
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_FREEZE))
return;
update_xaxis(self, msg->utime);
double timestamp = msg->utime * 1e-6;
bot_gl_scrollplot2d_add_point (self->navigator_rotation_plot, "control", timestamp,
msg->rotation_speed);
bot_gl_scrollplot2d_add_point (self->navigator_translation_plot, "control", timestamp,
msg->translation_speed);
viewer_request_redraw (self->viewer);
}
static void
car_draw (Viewer *viewer, Renderer *super)
{
RendererCar *self = (RendererCar*) super->user;
botlcm_pose_t pose;
if (!atrans_get_local_pose (self->atrans, &pose))
return;
BotGtkParamWidget *pw = self->pw;
int bling = self->chassis_model ?
bot_gtk_param_widget_get_bool (pw, PARAM_NAME_BLING) : 0;
int wheels = self->wheel_model ?
bot_gtk_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 (2);
glBegin(GL_LINE_STRIP);
glVertex3dv (self->last_pose.pos);
for (unsigned int i = 0;
i < MIN (bot_ptr_circular_size(self->path), self->max_draw_poses);
i++) {
glVertex3dv(bot_ptr_circular_index(self->path, i));
}
glEnd();
glPushMatrix();
// compute the rotation matrix to orient the vehicle in world
// coordinates
double body_quat_m[16];
bot_quat_pos_to_matrix(pose.orientation, pose.pos, body_quat_m);
// opengl expects column-major matrices
double body_quat_m_opengl[16];
bot_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];
bot_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;
}
}
glColor3f(1,1,1);
bot_gl_draw_text(pose.pos, GLUT_BITMAP_HELVETICA_12, buf,
BOT_GL_DRAW_TEXT_DROP_SHADOW);
}
}
Renderer *renderer_scrolling_plots_new (Viewer *viewer)
{
RendererScrollingPlots *self =
(RendererScrollingPlots*) calloc (1, sizeof (RendererScrollingPlots));
self->viewer = viewer;
self->renderer.draw = scrolling_plots_draw;
self->renderer.destroy = scrolling_plots_free;
self->renderer.name = "Scrolling Plots";
self->renderer.user = self;
self->renderer.enabled = 1;
self->renderer.widget = gtk_alignment_new (0, 0.5, 1.0, 0);
self->atrans = globals_get_atrans();
self->lcm = globals_get_lcm ();
self->pw = BOT_GTK_PARAM_WIDGET (bot_gtk_param_widget_new ());
gtk_container_add (GTK_CONTAINER (self->renderer.widget),
GTK_WIDGET(self->pw));
gtk_widget_show (GTK_WIDGET (self->pw));
bot_gtk_param_widget_add_int (self->pw, PARAM_NAME_SIZE,
BOT_GTK_PARAM_WIDGET_SLIDER, 50, 800, 10, 150);
bot_gtk_param_widget_add_double (self->pw, PARAM_NAME_GRAPH_TIMESPAN,
BOT_GTK_PARAM_WIDGET_SLIDER, 1, 20, 0.5, 5);
bot_gtk_param_widget_add_booleans (self->pw,
BOT_GTK_PARAM_WIDGET_TOGGLE_BUTTON, PARAM_NAME_FREEZE, 0, NULL);
bot_gtk_param_widget_add_booleans (self->pw, 0,
PARAM_NAME_RENDER_NAVIGATOR_ROTATION, 1,
PARAM_NAME_RENDER_NAVIGATOR_TRANSLATION, 1,
NULL);
bot_gtk_param_widget_add_booleans (self->pw, 0,
PARAM_NAME_RENDER_ENCODER_LEFT, 1,
PARAM_NAME_RENDER_ENCODER_RIGHT, 1, NULL);
bot_gtk_param_widget_add_booleans (self->pw, 0,
PARAM_NAME_SHOW_LEGEND, 0, NULL);
g_signal_connect (G_OBJECT (self->pw), "changed",
G_CALLBACK (on_param_widget_changed), self);
BotConf *config = globals_get_config ();
self->encoder_left_scale = 1.0;//bot_conf_get_double_or_fail (config, "calibration.encoders.WHEEL_LEFT.scale");
self->encoder_right_scale = 1.0;//bot_conf_get_double_or_fail (config, "calibration.encoders.WHEEL_RIGHT.scale");
globals_release_config (config);
self->encoder_left_last = 0;
self->encoder_right_last = 0;
// navigator rotation
self->navigator_rotation_plot = bot_gl_scrollplot2d_new ();
bot_gl_scrollplot2d_set_title (self->navigator_rotation_plot, "Nav. Rot. Speed");
bot_gl_scrollplot2d_set_text_color (self->navigator_rotation_plot, 0.7, 0.7, 0.7, 1);
bot_gl_scrollplot2d_set_bgcolor (self->navigator_rotation_plot, 0.1, 0.1, 0.1, 0.7);
bot_gl_scrollplot2d_set_border_color (self->navigator_rotation_plot, 1, 1, 1, 0.7);
bot_gl_scrollplot2d_set_ylim (self->navigator_rotation_plot, -.5, .5);
bot_gl_scrollplot2d_add_plot (self->navigator_rotation_plot, "control", 1000);
bot_gl_scrollplot2d_set_color (self->navigator_rotation_plot, "control", 0.7, 0, 0.7, 1);
bot_gl_scrollplot2d_add_plot (self->navigator_rotation_plot, "status", 1000);
bot_gl_scrollplot2d_set_color (self->navigator_rotation_plot, "status", 0, 0, 1, 1);
//bot_gl_scrollplot2d_add_plot (self->navigator_rotation_plot, "2500", 1000);
//bot_gl_scrollplot2d_set_color (self->navigator_rotation_plot, "2500", 0.8, 0.8, 0.8, 0.5);
self->navigator_translation_plot = bot_gl_scrollplot2d_new ();
bot_gl_scrollplot2d_set_title (self->navigator_translation_plot, "Nav. Trans. Speed");
bot_gl_scrollplot2d_set_text_color (self->navigator_translation_plot, 0.7, 0.7, 0.7, 1);
bot_gl_scrollplot2d_set_bgcolor (self->navigator_translation_plot, 0.1, 0.1, 0.1, 0.7);
bot_gl_scrollplot2d_set_border_color (self->navigator_translation_plot, 1, 1, 1, 0.7);
bot_gl_scrollplot2d_set_ylim (self->navigator_translation_plot, 0, 2.0);
bot_gl_scrollplot2d_add_plot (self->navigator_translation_plot, "control", 1000);
bot_gl_scrollplot2d_set_color (self->navigator_translation_plot, "control", 0.7, 0, 0.7, 1);
bot_gl_scrollplot2d_add_plot (self->navigator_translation_plot, "status", 1000);
bot_gl_scrollplot2d_set_color (self->navigator_translation_plot, "status", 0, 0, 1, 1);
// left wheel encoder plot
self->encoder_left_plot = bot_gl_scrollplot2d_new ();
bot_gl_scrollplot2d_set_title (self->encoder_left_plot, "Left Encoder");
bot_gl_scrollplot2d_set_text_color (self->encoder_left_plot, 0.7, 0.7, 0.7, 1);
bot_gl_scrollplot2d_set_border_color (self->encoder_left_plot, 1, 1, 1, 0.7);
bot_gl_scrollplot2d_set_bgcolor (self->encoder_left_plot, 0.1, 0.1, 0.1, 0.7);
bot_gl_scrollplot2d_set_ylim (self->encoder_left_plot, 0, 2.0);
bot_gl_scrollplot2d_add_plot (self->encoder_left_plot, "actual", 1000);
bot_gl_scrollplot2d_set_color (self->encoder_left_plot, "actual", 0.7, 0, 0.7, 1);
bot_gl_scrollplot2d_add_plot (self->encoder_left_plot, "0", 1000);
bot_gl_scrollplot2d_set_color (self->encoder_left_plot, "0", 0.8, 0.8, 0.8, 0.5);
// right wheel encoder plot
self->encoder_right_plot = bot_gl_scrollplot2d_new ();
bot_gl_scrollplot2d_set_title (self->encoder_right_plot, "Right Encoder");
bot_gl_scrollplot2d_set_text_color (self->encoder_right_plot, 0.7, 0.7, 0.7, 1);
bot_gl_scrollplot2d_set_border_color (self->encoder_right_plot, 1, 1, 1, 0.7);
bot_gl_scrollplot2d_set_bgcolor (self->encoder_right_plot, 0.1, 0.1, 0.1, 0.7);
bot_gl_scrollplot2d_set_ylim (self->encoder_right_plot, 0, 2.0);
bot_gl_scrollplot2d_add_plot (self->encoder_right_plot, "actual", 1000);
bot_gl_scrollplot2d_set_color (self->encoder_right_plot, "actual", 0.7, 0, 0.7, 1);
bot_gl_scrollplot2d_add_plot (self->encoder_right_plot, "0", 1000);
bot_gl_scrollplot2d_set_color (self->encoder_right_plot, "0", 0.8, 0.8, 0.8, 0.5);
// legends?
BotGlScrollPlot2dLegendLocation legloc = BOT_GL_SCROLLPLOT2D_HIDDEN;
if (bot_gtk_param_widget_get_bool (self->pw, PARAM_NAME_SHOW_LEGEND)) {
legloc = BOT_GL_SCROLLPLOT2D_TOP_RIGHT;
}
bot_gl_scrollplot2d_set_show_legend (self->navigator_rotation_plot, legloc);
bot_gl_scrollplot2d_set_show_legend (self->navigator_translation_plot, legloc);
// subscribe to LC messages
fblcm_motion_cmd_t_subscribe (self->lcm, "MOTION_CMD", on_motion_cmd, self);
//arlcm_encoder_t_subscribe (self->lcm, "WHEEL_LEFT", on_encoders, self);
//arlcm_encoder_t_subscribe (self->lcm, "WHEEL_RIGHT", on_encoders, self);
// periodically pull pose data from ATrans
//g_timeout_add (30, get_speed_update, self);
return &self->renderer;
}
