static void on_param_changed( GtkuParamWidget *pw, const char *name, void *user_data )
{
RendererTextured *self = (RendererTextured*) user_data;
self->numScans = gtku_param_widget_get_int(self->pw, PARAM_MEMORY);
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
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);
}
Renderer* renderer_textured_new( Viewer *viewer)
{
RendererTextured *self =
(RendererTextured*) calloc(1, sizeof(RendererTextured));
Renderer *renderer = (Renderer*) self;
// build color table
for (int i = 0; i < VELODYNE_NUM_LASERS; i++) {
float f[4];
srand(i);
color_util_rand_color(f, .6, .3);
self->laser_colors[i][0] = f[0];
self->laser_colors[i][1] = f[1];
self->laser_colors[i][2] = f[2];
}
renderer->draw = my_draw;
renderer->name = "Velodyne";
renderer->destroy = my_free;
renderer->widget = gtk_vbox_new ( FALSE, 0);
renderer->enabled = 1;
renderer->user = self;
self->pw = GTKU_PARAM_WIDGET (gtku_param_widget_new ());
self->circular = gu_ptr_circular_new(12000, free_velodyne_data, NULL);
self->viewer = viewer;
gtku_param_widget_add_int (self->pw, PARAM_MEMORY,
GTKU_PARAM_WIDGET_SLIDER, 1, 10, 1, 1);
self->numScans = gtku_param_widget_get_int(self->pw, PARAM_MEMORY);
gtku_param_widget_add_enum( self->pw, COLOR_MENU, GTKU_PARAM_WIDGET_MENU,
COLOR_Z,
"Drab", COLOR_DRAB,
"Height", COLOR_Z,
"Counted", COLOR_COUNTED,
"Intensity", COLOR_INTENSITY,
"Laser", COLOR_LASER,
"RGB (24 bit)", COLOR_RGB,
"Do Not Render", COLOR_NONE,
NULL);
gtk_box_pack_start (GTK_BOX (renderer->widget), GTK_WIDGET (self->pw),
FALSE, TRUE, 0);
gtk_widget_show (GTK_WIDGET (self->pw));
self->lc = globals_get_lcm();
self->ctrans = globals_get_ctrans();
self->calib = velodyne_calib_create();
lcmtypes_rgbvelodyne_t_subscribe(self->lc, "RGB_VELODYNE", on_velodyne, self);
g_signal_connect( G_OBJECT(self->pw), "changed", G_CALLBACK(on_param_changed), self);
g_signal_connect (G_OBJECT (viewer), "load-preferences",
G_CALLBACK (on_load_preferences), self);
g_signal_connect (G_OBJECT (viewer), "save-preferences",
G_CALLBACK (on_save_preferences), self);
return (Renderer*) self;
}
static void my_draw( Viewer *viewer, Renderer *renderer )
{
RendererRrtDebug *self = (RendererRrtDebug*) renderer->user;
if (!self->data) return;
double min0 = gtku_param_widget_get_double(self->pw, PARAM_MIN0);
double max0 = gtku_param_widget_get_double(self->pw, PARAM_MAX0);
int id_min = gtku_param_widget_get_int(self->pw, PARAM_MIN_PLOT);
int id_max = gtku_param_widget_get_int(self->pw, PARAM_MAX_PLOT);
if (max0 == MAX_ID_TO_PLOT) { // Plot everything
max0 = INT_MAX;
}
int64_t min_id_plot = self->min_node_id + min0 + id_min;
int64_t max_id_plot = self->min_node_id + max0 + id_max;
double p_local[3], z;
ctrans_local_pos (self->ctrans, p_local);
z = p_local[2] - 0.06;
// Controller input
if (self->plot_ctrl) {
glLineWidth (0.1);
// Forward and Reverse
for (int forward = 0; forward < 2; forward++) {
if (forward == 1)
glColor3f (0.439, 0.439, 0.439); // silver for forward
else
glColor3f (0.6, 0.4, 0.0); // gold for reverse
for( int i = 0; i < self->data->num_nodes_plot; i++ ) {
lcmtypes_rrt_debug_node_t *node = &self->data->node[i];
// skip out of range, other direction
if ((node->npt_input <= 1) || (node->is_forward != forward) ||
(node->id < min_id_plot) || (node->id > max_id_plot)) {
continue;
}
glBegin (GL_LINE_STRIP);
glVertex3f (node->input[0].x, node->input[0].y, z);
glVertex3f (node->input[1].x, node->input[1].y, z);
glEnd ();
}
}
}
// Predicted states
if (self->plot_state) {
glLineWidth (0.1);
double best_total_cost = DBL_MAX;
for( int i = 0; i < self->data->num_nodes_plot; i++ ) {
double UB_cost = self->data->node[i].UB_cost;
if (UB_cost > 0)
best_total_cost = MIN (best_total_cost,
UB_cost + self->data->node[i].cumulative_cost);
}
glEnable (GL_BLEND);
for( int i = 0; i < self->data->num_nodes_plot; i++ ) {
lcmtypes_rrt_debug_node_t *node = &self->data->node[i];
// skip out of range
if ((node->npt_state <= 1) ||
(node->id < min_id_plot) || (node->id > max_id_plot)) {
continue;
}
float alpha;
if (node->UB_cost < 0) { // Not reaching the target
alpha = 0.3;
if (node->is_forward)
glColor4f (0.75, 0.60, 0.50, alpha); // Khaki
else
glColor4f (0.13, 0.67, 0.66, alpha); // Sea green
}
else {
double ratio = best_total_cost /
(node->UB_cost + node->cumulative_cost);
alpha = MIN (MAX (0.2, ratio*ratio*ratio*ratio), 0.9);
if (node->is_forward)
glColor4f (0.5, 0.5, 1.0, alpha); // Purple
else
glColor4f (0.0, 0.4, 0.0, alpha); // Dark green
// Change color for the best ones
if (node->UB_cost + node->cumulative_cost < best_total_cost + 1) {
glColor4f(1, 1, 0, 0.9);
}
}
if (node->is_safe == 0) { // Unsafe node
glColor4f (0.9, 0.0, 0.0, alpha); // Red
}
glBegin (GL_LINE_STRIP);
for (int j = 0; j < node->npt_state; j++) {
glVertex3f (node->state[j].x, node->state[j].y, z);
}
glEnd ();
}
glDisable (GL_BLEND);
}
}
