static void
on_param_widget_changed (GtkuParamWidget *pw, const char *name,
RendererRrtDebug *self)
{
if (! strcmp (name, PARAM_NAME_RENDER_CTRL)) {
self->plot_ctrl
= gtku_param_widget_get_bool (self->pw, PARAM_NAME_RENDER_CTRL);
}
else if (! strcmp (name, PARAM_NAME_RENDER_STATE)) {
self->plot_state
= gtku_param_widget_get_bool (self->pw, PARAM_NAME_RENDER_STATE);
}
viewer_request_redraw (self->viewer);
}
static void
on_param_widget_changed (GtkuParamWidget *pw, const char *name, void *user)
{
RendererCar *self = (RendererCar*) user;
Viewer *viewer = self->viewer;
viewer->view_handler->follow_mode = 0;
if (gtku_param_widget_get_bool(pw, PARAM_FOLLOW_POS))
viewer->view_handler->follow_mode |= FOLLOW_POS;
if (gtku_param_widget_get_bool(pw, PARAM_FOLLOW_YAW))
viewer->view_handler->follow_mode |= FOLLOW_YAW;
self->max_draw_poses = gtku_param_widget_get_int(pw, PARAM_MAXPOSES);
viewer_request_redraw ( self->viewer);
}
static void
on_param_changed( GtkuParamWidget *pw, const char *name, void *user_data )
{
if( ! strcmp( name, "Integer 1" ) ) {
printf("int1: %d\n", gtku_param_widget_get_int( pw, name ) );
} else if ( ! strcmp( name, "Integer Slider" ) ) {
printf("int slider: %d\n", gtku_param_widget_get_int( pw, name ) );
} else if ( ! strcmp( name, "Double Slider" ) ) {
printf("double slider: %f\n", gtku_param_widget_get_double( pw, name ) );
} else if ( ! strcmp( name, "check1" ) ||
! strcmp( name, "check2" ) ||
! strcmp( name, "check3" ) ) {
printf("%s: %s\n", name,
gtku_param_widget_get_bool( pw, name ) ? "True" : "False" );
} else if( ! strcmp( name, "Menu" ) ) {
printf("menu: %s / %d\n",
gtku_param_widget_get_enum_str( pw, name ),
gtku_param_widget_get_enum( pw, name ) );
}
}
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);
}
}
