Renderer* renderer_velodyne_new( Viewer *viewer)
{
RendererVelodyne *self =
(RendererVelodyne*) calloc(1, sizeof(RendererVelodyne));
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(1000, free_velodyne_data, NULL);
self->viewer = viewer;
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,
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_velodyne_t_subscribe(self->lc, "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;
}
void setup_renderer_rrt_debug(Viewer *viewer, int priority)
{
RendererRrtDebug *self =
(RendererRrtDebug*) calloc(1, sizeof(RendererRrtDebug));
Renderer *renderer = &self->renderer;
renderer->draw = my_draw;
renderer->destroy = my_free;
renderer->name = "RRT Debug";
renderer->widget = gtk_vbox_new(FALSE, 0);
renderer->enabled = 1;
renderer->user = self;
self->lc = globals_get_lcm();
self->ctrans = globals_get_ctrans();
self->viewer = viewer;
self->data = NULL;
self->min_node_id = 0;
self->plot_ctrl = 0;
self->plot_state = 1;
self->pw = GTKU_PARAM_WIDGET(gtku_param_widget_new());
gtk_box_pack_start(GTK_BOX(renderer->widget), GTK_WIDGET(self->pw), TRUE, TRUE, 0);
gtku_param_widget_add_double (self->pw, PARAM_MIN0,
GTKU_PARAM_WIDGET_SLIDER, 0, 1e4, 1e2, 0);
gtku_param_widget_add_int (self->pw, PARAM_MIN_PLOT,
GTKU_PARAM_WIDGET_SLIDER, 0, 5e2, 1, 0);
gtku_param_widget_add_double (self->pw, PARAM_MAX0,
GTKU_PARAM_WIDGET_SLIDER, 0, MAX_ID_TO_PLOT, 1e2, MAX_ID_TO_PLOT);
gtku_param_widget_add_int (self->pw, PARAM_MAX_PLOT,
GTKU_PARAM_WIDGET_SLIDER, 0, 5e2, 1, 5e2);
gtku_param_widget_add_booleans (self->pw, 0,
PARAM_NAME_RENDER_CTRL, self->plot_ctrl,
PARAM_NAME_RENDER_STATE, self->plot_state, NULL);
gtk_widget_show_all(renderer->widget);
g_signal_connect (G_OBJECT (self->pw), "changed",
G_CALLBACK (on_param_widget_changed), self);
lcmtypes_rrt_debug_t_subscribe(self->lc, "RRT_DEBUG", on_rrt_debug, self);
viewer_add_renderer(viewer, renderer, priority);
}
int main(int argc, char **argv)
{
printf("param_widget_test\n");
GtkWidget *window;
GtkuParamWidget *pw;
gtk_init( &argc, &argv );
window = gtk_window_new( GTK_WINDOW_TOPLEVEL );
g_signal_connect( G_OBJECT(window), "delete_event", gtk_main_quit, NULL );
g_signal_connect( G_OBJECT(window), "destroy", gtk_main_quit, NULL );
gtk_container_set_border_width( GTK_CONTAINER(window), 10 );
pw = GTKU_PARAM_WIDGET( gtku_param_widget_new() );
gtk_container_add( GTK_CONTAINER(window), GTK_WIDGET( pw ) );
gtku_param_widget_add_int( pw, "Integer 1", GTKU_PARAM_WIDGET_SPINBOX,
INT_MIN, INT_MAX, 1, 10 );
gtku_param_widget_add_int( pw, "Integer Slider", GTKU_PARAM_WIDGET_SLIDER,
0, 100, 50, 1 );
gtku_param_widget_add_double( pw, "Double Slider", GTKU_PARAM_WIDGET_SLIDER,
0, 1, 0.01, 0.5 );
gtku_param_widget_add_booleans( pw, GTKU_PARAM_WIDGET_CHECKBOX,
"check1", 0,
"check2", 1,
"check3", 0, NULL );
gtku_param_widget_add_enum( pw, "Menu", GTKU_PARAM_WIDGET_MENU,
MENU_1,
"entry 1", MENU_1,
"entry 2", MENU_2,
"entry 3", MENU_3,
NULL );
g_signal_connect( G_OBJECT( pw ), "changed",
G_CALLBACK( on_param_changed ), NULL );
gtk_widget_show_all( window );
gtk_main();
return 0;
}
static void my_draw( Viewer *viewer, Renderer *renderer )
{
RendererTextured *self = (RendererTextured*) renderer->user;
GtkuParamWidget *pw = GTKU_PARAM_WIDGET (self->pw);
int colormode = gtku_param_widget_get_enum(pw, COLOR_MENU);
if(colormode == COLOR_NONE)
return;
double total_theta = 0;
double last_ctheta = HUGE;
double pos[3];
if (!ctrans_have_pose(self->ctrans))
return;
ctrans_local_pos(self->ctrans, pos);
glEnable(GL_DEPTH_TEST);
glPointSize(2.0);
glBegin(GL_POINTS);
for (unsigned int cidx = 0; cidx < gu_ptr_circular_size(self->circular); cidx++) {
struct velodyne_data *vdata = (struct velodyne_data*) gu_ptr_circular_index(self->circular, cidx);
// do these returns still need to be projected
if (vdata->samples == NULL) {
vdata->samples = (velodyne_sample_t*) calloc(velodyne_decoder_estimate_samples(self->calib, vdata->data, vdata->datalen),
sizeof(velodyne_sample_t));
velodyne_decoder_t vdecoder;
velodyne_decoder_init(self->calib, &vdecoder, vdata->data, vdata->datalen);
while (!velodyne_decoder_next(self->calib, &vdecoder, &vdata->samples[vdata->num_samples])) {
velodyne_sample_t *vsample = &vdata->samples[vdata->num_samples++];
if (vsample->range < 0.25 && fabs(mod2pi(vsample->theta+M_PI/2)) < to_radians(20)) {
bad_samples[vsample->logical]++;
}
total_ranges[vsample->logical]++;
if (vsample->range < 0.01)
bad_ranges[vsample->logical]++;
double sensor_xyz[4] = { vsample->xyz[0], vsample->xyz[1], vsample->xyz[2], 1 };
double local_xyz[4];
matrix_vector_multiply_4x4_4d(vdata->m, sensor_xyz, local_xyz);
vsample->xyz[0] = local_xyz[0];
vsample->xyz[1] = local_xyz[1];
vsample->xyz[2] = local_xyz[2];
vdata->ctheta = vsample->ctheta;
/*
int thissign = sgn(mod2pi(vsample.ctheta));
if (thissign==1 && lastsign==-1) {
rotation_count++;
int ROTATION_COUNT_THRESH = 5;
if (rotation_count == ROTATION_COUNT_THRESH) {
double elapsed_time = (v->utime - rotation_utime)/1000000.0;
printf("velodyne %6s [%8d]: %5.3f Hz\n",
vdecoder.version_string, vdecoder.revolution_count,
rotation_count / elapsed_time);
rotation_count = 0;
rotation_utime = v->utime;
if (0) {
for (int i = 0; i < 64; i++)
printf("%3d : %6f\n", i, ((double)bad_ranges[i]*100.0)/total_ranges[i]);
}
}
}
*/
}
}
// have we plotted a whole revolution of data?
if (last_ctheta == HUGE)
last_ctheta = vdata->ctheta;
total_theta += fabs(mod2pi(last_ctheta - vdata->ctheta));
if (total_theta > self->numScans*2*M_PI)
break;
last_ctheta = vdata->ctheta;
//do motion comensation
//struct velodyne_data motion_compensated;
//motion_compensated.samples = (velodyne_sample_t*)malloc(vdata->num_samples);
//motion_compensate_scan(vdata, self, &motion_compensated);
// project these samples
for (unsigned int s = 0; s < vdata->num_samples; s++) {
velodyne_sample_t *vsample = &vdata->samples[s];
if(vsample->range > RANGE_THRESH)
{
switch (colormode)
{
case COLOR_INTENSITY:
{
double v = vsample->intensity;
glColor3fv(color_util_jet(v));
break;
}
case COLOR_RGB:
{
double rgb[3];
rgb[0] = (double)(vdata->rgbdata->rgb[s][0])/255;
rgb[1] = (double)(vdata->rgbdata->rgb[s][1])/255;
rgb[2] = (double)(vdata->rgbdata->rgb[s][2])/255;
glColor3dv(rgb);
break;
}
case COLOR_LASER:
glColor3d(self->laser_colors[vsample->logical][0],
self->laser_colors[vsample->logical][1],
self->laser_colors[vsample->logical][2]);
break;
case COLOR_DRAB:
glColor3d(0.3, 0.3, 0.3);
break;
case COLOR_Z:
{
double z = vsample->xyz[2] - pos[2];
double Z_MIN = -1, Z_MAX = 2;
double z_norm = (z - Z_MIN) / (Z_MAX - Z_MIN);
glColor3fv(color_util_jet(z_norm));
break;
}
case COLOR_COUNTED:
{
switch (vsample->logical%10)
{
case 0:
glColor3d(1, 1, 0.5); break;
case 5:
glColor3d(0.5, 1, 1); break;
default:
glColor3d(0.3, 0.3, 0.3); break;
}
}
break;
}
glVertex3dv(vsample->xyz);
}
}
}
glEnd();
}
void setup_renderer_car(Viewer *viewer, int render_priority)
{
RendererCar *self = (RendererCar*) calloc (1, sizeof (RendererCar));
Renderer *renderer = &self->renderer;
renderer->draw = car_draw;
renderer->destroy = car_free;
renderer->widget = gtk_vbox_new(FALSE, 0);
renderer->name = RENDERER_NAME;
renderer->user = self;
renderer->enabled = 1;
EventHandler *ehandler = &self->ehandler;
ehandler->name = RENDERER_NAME;
ehandler->enabled = 1;
ehandler->pick_query = pick_query;
ehandler->key_press = key_press;
ehandler->hover_query = pick_query;
ehandler->mouse_press = mouse_press;
ehandler->mouse_release = mouse_release;
ehandler->mouse_motion = mouse_motion;
ehandler->user = self;
self->viewer = viewer;
self->lc = globals_get_lcm ();
self->config = globals_get_config ();
self->pw = GTKU_PARAM_WIDGET(gtku_param_widget_new());
self->ctrans = globals_get_ctrans ();
self->path = gu_ptr_circular_new (MAX_POSES, free_path_element, NULL);
gtk_box_pack_start(GTK_BOX(renderer->widget), GTK_WIDGET(self->pw), TRUE,
TRUE, 0);
gtku_param_widget_add_booleans (self->pw, 0, PARAM_FOLLOW_POS, 1, NULL);
gtku_param_widget_add_booleans (self->pw, 0, PARAM_FOLLOW_YAW, 0, NULL);
char * model;
char path[256];
if (config_get_str (self->config, "renderer_car.chassis_model",
&model) == 0) {
snprintf (path, sizeof (path), "%s/%s", MESH_MODEL_PATH, model);
self->chassis_model = rwx_model_create (path);
}
if (config_get_str (self->config, "renderer_car.wheel_model",
&model) == 0) {
snprintf (path, sizeof (path), "%s/%s", MESH_MODEL_PATH, model);
self->wheel_model = rwx_model_create (path);
}
if (self->chassis_model)
gtku_param_widget_add_booleans (self->pw, 0, PARAM_NAME_BLING, 1,
NULL);
if (self->wheel_model)
gtku_param_widget_add_booleans (self->pw, 0, PARAM_NAME_WHEELS, 1,
NULL);
self->max_draw_poses = 1000;
gtku_param_widget_add_int (self->pw, PARAM_MAXPOSES,
GTKU_PARAM_WIDGET_SLIDER, 0, MAX_POSES, 100, self->max_draw_poses);
GtkWidget *find_button = gtk_button_new_with_label("Find");
gtk_box_pack_start(GTK_BOX(renderer->widget), find_button, FALSE, FALSE, 0);
g_signal_connect(G_OBJECT(find_button), "clicked",
G_CALLBACK (on_find_button), self);
GtkWidget *clear_button = gtk_button_new_with_label("Clear path");
gtk_box_pack_start(GTK_BOX(renderer->widget), clear_button, FALSE, FALSE,
0);
g_signal_connect(G_OBJECT(clear_button), "clicked",
G_CALLBACK (on_clear_button), self);
gtk_widget_show_all(renderer->widget);
g_signal_connect (G_OBJECT (self->pw), "changed",
G_CALLBACK (on_param_widget_changed), self);
on_param_widget_changed(self->pw, "", self);
ctrans_subscribe (self->ctrans, pose_listener, self);
#ifdef USE_CANDECODE
self->can = can_decode_new (self->lc);
can_decode_subscribe (self->can, CAN_MSG_WHEELSPEEDS, on_wheelspeeds, self);
can_decode_subscribe (self->can, CAN_MSG_STEERING_WHEEL,
on_steering, self);
can_decode_subscribe (self->can, CAN_MSG_SUSPENSION_HEIGHT,
on_suspension, self);
#endif
viewer_add_renderer(viewer, &self->renderer, render_priority);
viewer_add_event_handler(viewer, &self->ehandler, render_priority);
self->footprint = config_util_get_vehicle_footprint (self->config);
assert (self->footprint);
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);
}
static void
on_image (const lcm_recv_buf_t *rbuf, const char *channel,
const lcmtypes_image_t *msg, void *user_data)
{
RendererCamThumb *self = (RendererCamThumb*) user_data;
if (! self->renderer.enabled) return;
cam_renderer_t *cr = g_hash_table_lookup (self->cam_handlers, channel);
if (!cr) {
cr = (cam_renderer_t*) calloc (1, sizeof (cam_renderer_t));
cr->renderer = self;
cr->render_place = 0;
cr->channel = strdup (channel);
g_hash_table_replace (self->cam_handlers, cr->channel, cr);
}
if (! cr->msg_received) {
cr->gl_area = GTKU_GL_DRAWING_AREA (gtku_gl_drawing_area_new (FALSE));
cr->pw = GTKU_PARAM_WIDGET (gtku_param_widget_new ());
gtku_param_widget_add_enum (cr->pw, PARAM_RENDER_IN,
0,
cr->render_place,
"Here", RENDER_IN_WIDGET,
"Top Left", RENDER_IN_TOP_LEFT,
"Top Cent.", RENDER_IN_TOP_CENTER,
"Top Right", RENDER_IN_TOP_RIGHT,
"Bot. Left", RENDER_IN_BOTTOM_LEFT,
"Bot. Cent.", RENDER_IN_BOTTOM_CENTER,
"Bot. Right", RENDER_IN_BOTTOM_RIGHT,
NULL);
cr->expander = gtk_expander_new (channel);
gtk_box_pack_start (GTK_BOX (self->renderer.widget),
cr->expander, TRUE, TRUE, 0);
GtkWidget *vbox = gtk_vbox_new (FALSE, 0);
gtk_container_add (GTK_CONTAINER (cr->expander), vbox);
gtk_box_pack_start (GTK_BOX (vbox),
GTK_WIDGET (cr->pw), TRUE, TRUE, 0);
gtk_box_pack_start (GTK_BOX (vbox),
GTK_WIDGET (cr->gl_area), TRUE, TRUE, 0);
g_signal_connect (G_OBJECT (cr->gl_area), "size-allocate",
G_CALLBACK (on_gl_area_size), cr);
cr->width = msg->width;
cr->height = msg->height;
gtk_widget_show_all (GTK_WIDGET (cr->expander));
gtk_expander_set_expanded (GTK_EXPANDER (cr->expander), cr->expanded);
if (cr->render_place == RENDER_IN_WIDGET) {
gtk_widget_show (GTK_WIDGET (cr->gl_area));
} else {
gtk_widget_hide (GTK_WIDGET (cr->gl_area));
}
g_signal_connect (G_OBJECT (cr->pw), "changed",
G_CALLBACK (on_cam_renderer_param_widget_changed), cr);
g_signal_connect (G_OBJECT (cr->gl_area), "expose-event",
G_CALLBACK (on_gl_area_expose), cr);
g_signal_connect (G_OBJECT (cr->expander), "notify::expanded",
G_CALLBACK (on_expander_expanded), cr);
cr->texture = NULL;
cr->last_image = NULL;
cr->renderer = self;
cr->uncompressed_buffer_size = msg->width * msg->height * 3;
cr->uncompresed_buffer =
(uint8_t*) malloc (cr->uncompressed_buffer_size);
cr->msg_received = 1;
}
if (cr->last_image) {
lcmtypes_image_t_destroy (cr->last_image);
}
cr->last_image = lcmtypes_image_t_copy (msg);
cr->is_uploaded = 0;
switch (gtku_param_widget_get_enum (cr->pw, PARAM_RENDER_IN)) {
case RENDER_IN_WIDGET:
if (gtk_expander_get_expanded (GTK_EXPANDER (cr->expander)))
gtku_gl_drawing_area_invalidate (cr->gl_area);
default:
viewer_request_redraw (self->viewer);
break;
}
}
void setup_renderer_simtraffic(Viewer *viewer, int render_priority)
{
RendererSimTraffic *self =
(RendererSimTraffic*) calloc (1, sizeof (RendererSimTraffic));
// renderer administrativa
Renderer *renderer = &self->renderer;
renderer->draw = simtraffic_draw;
renderer->destroy = simtraffic_free;
renderer->widget = gtk_vbox_new(FALSE, 0);
renderer->name = "SimTraffic";
renderer->user = self;
renderer->enabled = 1;
// event handler
EventHandler *ehandler = &self->ehandler;
ehandler->name = "SimTraffic";
ehandler->enabled = 1;
ehandler->pick_query = pick_query;
ehandler->hover_query = pick_query;
ehandler->mouse_press = mouse_press;
ehandler->mouse_motion = mouse_motion;
ehandler->mouse_release = mouse_release;
ehandler->key_press = key_press;
ehandler->user = self;
// member variables
self->viewer = viewer;
self->lc = globals_get_lcm();
self->ctrans = globals_get_ctrans();
self->tracks = NULL;
self->lchandler = lcmtypes_track_list_t_subscribe (self->lc,
"LIDAR_TRACKS_SIM", on_track_list, self);
// parameter widget
self->pw = GTKU_PARAM_WIDGET(gtku_param_widget_new());
g_signal_connect (G_OBJECT (self->pw), "changed",
G_CALLBACK (on_param_widget_changed), self);
on_param_widget_changed(self->pw, "", self);
gtk_box_pack_start(GTK_BOX(renderer->widget), GTK_WIDGET(self->pw),
TRUE, TRUE, 0);
// add car button
GtkWidget *add_bt = gtk_button_new_with_label ("Add car");
gtk_box_pack_start (GTK_BOX (renderer->widget), add_bt,
FALSE, FALSE, 0);
gtk_widget_show (add_bt);
g_signal_connect (G_OBJECT(add_bt), "clicked",
G_CALLBACK(on_add_bt_clicked), self);
gtk_widget_show_all(renderer->widget);
// popup menu
self->context_menu = GTK_MENU (gtk_menu_new ());
g_object_ref_sink (self->context_menu);
GtkWidget *pause_mi = gtk_menu_item_new_with_label ("Pause");
GtkWidget *unpause_mi = gtk_menu_item_new_with_label ("Unpause");
gtk_menu_shell_append (GTK_MENU_SHELL(self->context_menu), pause_mi);
gtk_menu_shell_append (GTK_MENU_SHELL(self->context_menu), unpause_mi);
g_signal_connect (G_OBJECT(pause_mi), "activate",
G_CALLBACK(on_pause_mi_activate), self);
g_signal_connect (G_OBJECT(unpause_mi), "activate",
G_CALLBACK(on_unpause_mi_activate), self);
gtk_widget_show_all (GTK_WIDGET (self->context_menu));
// administrativa
viewer_add_renderer(viewer, renderer, render_priority);
viewer_add_event_handler(viewer, ehandler, render_priority);
}
