Beispiel #1
0
static void
on_cam_renderer_param_widget_changed (GtkuParamWidget *pw, const char *param,
        void *user_data)
{
    cam_renderer_t *cr = (cam_renderer_t*) user_data;

    // delete the old texture object if it exists.  make sure that we've
    // selected the correct OpenGL context
    if (cr->texture) {
        if (cr->render_place == RENDER_IN_WIDGET) {
            gtku_gl_drawing_area_set_context (cr->gl_area);
        } else {
            gtku_gl_drawing_area_set_context (cr->renderer->viewer->gl_area);
        }

        glutil_texture_free (cr->texture);
        cr->texture = NULL;
    }

    cr->render_place = gtku_param_widget_get_enum (pw, PARAM_RENDER_IN);
    if (cr->render_place == RENDER_IN_WIDGET) {
        gtk_widget_show (GTK_WIDGET (cr->gl_area));
    } else {
        gtk_widget_hide (GTK_WIDGET (cr->gl_area));
    }

    cr->is_uploaded = 0;
    viewer_request_redraw (cr->renderer->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 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();
}
Beispiel #4
0
static void
cam_thumb_draw (Viewer *viewer, Renderer *renderer)
{
    RendererCamThumb *self = (RendererCamThumb*) renderer->user;
    GLint viewport[4];

    glGetIntegerv (GL_VIEWPORT, viewport);

    // transform into window coordinates, where <0, 0> is the top left corner
    // of the window and <viewport[2], viewport[3]> is the bottom right corner
    // of the window
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    gluOrtho2D(0, viewport[2], 0, viewport[3]);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();
    glTranslatef(0, viewport[3], 0);
    glScalef(1, -1, 1);

    double vp_width = viewport[2] - viewport[0];
    double vp_height = viewport[3] - viewport[1];

    GPtrArray *crlist = gu_hash_table_get_vals (self->cam_handlers);
    for (int criter = 0; criter < g_ptr_array_size(crlist); criter++) {
        cam_renderer_t *cr = g_ptr_array_index(crlist, criter);

        if (!cr->last_image) continue;
        double aspect = cr->last_image->width / 
            (double) cr->last_image->height;

        double thumb_width, thumb_height;
        if ((vp_width / 3) / aspect > vp_height / 3) {
            thumb_height = vp_height / 3;
            thumb_width = thumb_height * aspect;
        } else {
            thumb_width = vp_width / 3;
            thumb_height = thumb_width / aspect;
        }

        int rmode = gtku_param_widget_get_enum (cr->pw, PARAM_RENDER_IN);
        if (rmode == RENDER_IN_WIDGET) continue;

        point2d_t p1 = { viewport[0], viewport[1] }; 

        switch (rmode) {
            case RENDER_IN_BOTTOM_RIGHT:
                p1.x = vp_width - thumb_width;
                p1.y = vp_height - thumb_height;
                break;
            case RENDER_IN_BOTTOM_CENTER:
                p1.x = vp_width / 3;
                p1.y = vp_height - thumb_height;
                break;
            case RENDER_IN_BOTTOM_LEFT:
                p1.x = 0;
                p1.y = vp_height - thumb_height;
                break;
            case RENDER_IN_TOP_LEFT:
                p1.x = 0;
                p1.y = 0;
                break;
            case RENDER_IN_TOP_CENTER:
                p1.x = vp_width / 3;
                p1.y = 0;
                break;
            case RENDER_IN_TOP_RIGHT:
                p1.x = vp_width - thumb_width;
                p1.y = 0;
                break;
            default:
                break;
        }

        glPushMatrix ();
        glTranslatef (p1.x, p1.y, 1);
        glScalef (thumb_width, thumb_height, 1);
        cam_renderer_draw (cr);
        glPopMatrix ();
    }
    g_ptr_array_free (crlist, TRUE);

    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();
}
Beispiel #5
0
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;
    }
}