コード例 #1
0
ファイル: ptr_circular.c プロジェクト: hchengwang/libbot
void bot_ptr_circular_resize(BotPtrCircular *circ, unsigned int new_capacity)
{
    // nothing to do?
    if (new_capacity == circ->capacity)
        return;

    // strategy: create a new circular buffer that contains the required data,
    // then swap out our internal representation. We have to be careful
    // about freeing any unneeded data.

    BotPtrCircular *newcirc = bot_ptr_circular_new(new_capacity, circ->handler, circ->user);

    // how many elements in the new one?
    // (so that we are guaranteed to never evict an element while
    // we populate the new data structure)
    unsigned int new_size = circ->size < new_capacity ? circ->size : new_capacity;

    // copy the data into the new structure, oldest first.
    for (unsigned int i = 0; i < new_size; i++)
        bot_ptr_circular_add(newcirc, bot_ptr_circular_index(circ, new_size - 1 - i));

    // free any elements we didn't copy
    for (unsigned int i = new_size; i < circ->size; i++) {
        void *p = bot_ptr_circular_index(circ, i);
        circ->handler(circ->user, p);
    }

    // okay, switch over the data structure
    if (circ->p)
        free(circ->p);
    memcpy(circ, newcirc, sizeof(BotPtrCircular));

    // free the new circ container, but not its storage
    free(newcirc);
}
コード例 #2
0
static int save_points_to_file(RendererLaser *self, FILE *file)
{
  /* first line: number of columns */
  fprintf(file, "%%%% %d\n", 10);
  /* second line: column titles */
  fprintf(file, "%%%% POINT_X POINT_Y POINT_Z ORIGIN_X ORIGIN_Y ORIGIN_Z"
      " POINT_STATUS LASER_NUMER SCAN_NUMER POINT_NUMBER\n");

  int count = 0;
  for (int chan_idx = 0; chan_idx < self->channels->len; chan_idx++) {
    laser_channel *lchan = g_ptr_array_index(self->channels, chan_idx);

    int scan_count = MIN(self->param_scan_memory, bot_ptr_circular_size(lchan->scans));
    for (int scan_idx = 0; scan_idx < scan_count; scan_idx++) {
      laser_projected_scan *lscan = bot_ptr_circular_index(lchan->scans, scan_idx);

      for (int i = 0; i < lscan->npoints; i++) {
        fprintf(file, "%0.3f %0.3f %0.3f %0.3f %0.3f %0.3f %d %d %d "
            "%d\n", lscan->points[i].x, lscan->points[i].y, lscan->points[i].z, lscan->origin.trans_vec[0],
            lscan->origin.trans_vec[1], lscan->origin.trans_vec[2], lscan->point_status[i], chan_idx, scan_idx, i);
        count++;
      }
    }
  }
  return count;
}
コード例 #3
0
ファイル: ptr_circular.c プロジェクト: hchengwang/libbot
void bot_ptr_circular_clear(BotPtrCircular *circ)
{
    for (unsigned int i = 0; i < circ->size; i++) {
        void *p = bot_ptr_circular_index(circ, i);
        circ->handler(circ->user, p);
    }

    circ->size = 0;
}
コード例 #4
0
ファイル: renderer_car.c プロジェクト: Patrick6289/navguide
static void
on_pose(const lcm_recv_buf_t *rbuf, const char *channel, 
        const botlcm_pose_t *pose, void *user_data)
{
    RendererCar *self = (RendererCar*) user_data;
    ViewHandler *vhandler = self->viewer->view_handler;

    double lastpos[3] = {0,0,0};
    if (bot_ptr_circular_size(self->path))
        memcpy(lastpos, bot_ptr_circular_index(self->path, 0),
               3 * sizeof(double));

    double diff[3];
    bot_vector_subtract_3d(pose->pos, lastpos, diff);

    if (bot_vector_magnitude_3d(diff) > 2.0) {
        // clear the buffer if we jump
        bot_ptr_circular_clear(self->path);
    }

    if (bot_vector_magnitude_3d(diff) > 0.1 ||
            bot_ptr_circular_size(self->path)==0) {
        double *p = (double*) calloc(3, sizeof(double));
        memcpy(p, pose->pos, sizeof(double)*3);
        bot_ptr_circular_add(self->path, p);
    }

    if (vhandler && vhandler->update_follow_target && !self->teleport_car) {
        vhandler->update_follow_target(vhandler, pose->pos, pose->orientation);
    }

    if (!self->did_teleport)
        on_find_button(NULL, self);
    self->did_teleport = 1;

    int64_t dt = pose->utime - self->last_pose.utime;
    double r = bot_conf_get_double_or_default (self->config,
            "renderer_car.wheel_radius", 0.3);

    if (self->last_pose.utime) {
        int i;
        for (i = 0; i < 4; i++) {
            self->wheelpos[i] += self->wheelspeeds[i] * dt * 1e-6 / r;
            self->wheelpos[i] = bot_mod2pi (self->wheelpos[i]);
        }
    }
    memcpy (&self->last_pose, &pose, sizeof (botlcm_pose_t));

    viewer_request_redraw(self->viewer);
}
コード例 #5
0
static void on_laser(const lcm_recv_buf_t *rbuf, const char *channel, const bot_core_planar_lidar_t *msg, void *user)
{
  RendererLaser *self = (RendererLaser*) user;
  g_assert(self);

  /* get the laser channel object based on channel name */
  laser_channel *lchan = g_hash_table_lookup(self->channels_hash, channel);
  if (lchan == NULL) {
    /* allocate and initialize laser channel structure */
    lchan = (laser_channel*) calloc(1, sizeof(laser_channel));
    g_assert(lchan);
    lchan->enabled = 1;
    lchan->name = bot_param_get_planar_lidar_name_from_lcm_channel(self->bot_param, channel);
    lchan->channel = strdup(channel);
    lchan->projector = laser_projector_new(self->bot_param, self->bot_frames, lchan->name, 1);

    char param_prefix[1024];
    bot_param_get_planar_lidar_prefix(NULL, lchan->name, param_prefix, sizeof(param_prefix));

    char color_key[1024];
    sprintf(color_key, "%s.viewer_color", param_prefix);
    double color[3];
    int color_size = bot_param_get_double_array(self->bot_param, color_key, color, 3);
    if (color_size != 3) {
      ERR("Error: Missing or funny color for planar LIDAR "
      "configuration key: '%s'\n", color_key);
      lchan->color[0] = 1;
      lchan->color[1] = 1;
      lchan->color[2] = 1;
    }
    else {
      lchan->color[0] = color[0];
      lchan->color[1] = color[1];
      lchan->color[2] = color[2];
    }

    lchan->scans = bot_ptr_circular_new(self->param_max_buffer_size, laser_scan_destroy, NULL);
    g_assert(lchan->scans);

    /* add laser channel to hash table and array */
    g_hash_table_insert(self->channels_hash, lchan->channel, lchan);
    g_ptr_array_add(self->channels, lchan);

    /* add check box */
    if (self->viewer)
      bot_gtk_param_widget_add_booleans(self->pw, 0, lchan->name, lchan->enabled, NULL);
  }

  /* TODO: Optimization - allocate space for local points from a
   circular buffer instead of calling calloc for each scan */

  laser_projected_scan *lscan = laser_create_projected_scan_from_planar_lidar(lchan->projector, msg,
      bot_frames_get_root_name(self->bot_frames));
  if (lscan == NULL)
    return; //probably didn't have a pose message yet...
  
  if(lchan->scans->capacity != self->param_max_buffer_size){
      bot_ptr_circular_resize(lchan->scans, self->param_max_buffer_size);
  }
  
  if (bot_ptr_circular_size(lchan->scans) > 0) {
    laser_projected_scan *last_scan = bot_ptr_circular_index(lchan->scans, 0);

    /* check for a large time difference between scans (typical when
     jumping around an LCM log file) */
    gboolean time_jump = FALSE;
    int64_t dt = msg->utime - last_scan->utime;
    if (dt < -OLD_HISTORY_THRESHOLD || dt > OLD_HISTORY_THRESHOLD)
      time_jump = TRUE;

    /* spacial decimation */
    gboolean stationary = FALSE;
    BotTrans delta;
    if (self->param_spacial_decimate && self->param_scan_memory > 10) {
      bot_trans_invert_and_compose(&lscan->origin, &last_scan->origin, &delta);
      double dist = bot_vector_magnitude_3d(delta.trans_vec);
      double rot;
      double axis[3];
      bot_quat_to_angle_axis(delta.rot_quat, &rot, axis);
      if (dist < SPACIAL_DECIMATION_LIMIT && rot < ANGULAR_DECIMATION_LIMIT) {
        stationary = TRUE;
      }
    }

    if (stationary) {
      laser_scan_destroy(NULL, lscan);
      return;
    }
  }

  bot_ptr_circular_add(lchan->scans, lscan);

  if (self->viewer)
    bot_viewer_request_redraw(self->viewer);

  return;
}
コード例 #6
0
static void renderer_laser_draw(BotViewer *viewer, BotRenderer *renderer)
{
  RendererLaser *self = (RendererLaser*) renderer->user;
  g_assert(self);

  /* get local position in order to height-color points */
  color_mode_t color_mode = self->param_color_mode;
  int z_relative = self->z_relative;
  double z_norm_scale = 0;
  if (color_mode == COLOR_MODE_Z) {
    if (!bot_frames_have_trans(self->bot_frames, "body", bot_frames_get_root_name(self->bot_frames))) {

      color_mode = COLOR_MODE_DRAB; /* no position */
    }
    else
      z_norm_scale = 1 / (self->param_color_mode_z_max_z - self->param_color_mode_z_min_z);
  }

  //compute total number of points that need to get drawn
  int numPointsToDraw = 0;
  for (int chan_idx = 0; chan_idx < self->channels->len; chan_idx++) {
    laser_channel *lchan = g_ptr_array_index(self->channels, chan_idx);
    if (!lchan->enabled)
      continue;
    int scan_count = MIN(self->param_scan_memory, bot_ptr_circular_size(lchan->scans));
    for (int scan_idx = 0; scan_idx < scan_count; scan_idx++) {
      laser_projected_scan *lscan = bot_ptr_circular_index(lchan->scans, scan_idx);
      if (!lscan->projection_status) {
        laser_update_projected_scan(lchan->projector, lscan, bot_frames_get_root_name(self->bot_frames));
      }
      //count number of points we want to draw
      for (int i = 0; i < lscan->npoints; i++) {
        if ((lscan->point_status[i]<=laser_valid_projection && lscan->points[i].z >= self->param_min_draw_z&& lscan->points[i].z <= self->param_max_draw_z)
              &&  (lscan->rawScan->ranges[i] <= self->param_max_draw_range ))

          numPointsToDraw++;
      }
    }
  }
  //reallocate the vertex buffers if necessary
  if (numPointsToDraw > self->pointBuffSize) {
    self->pointBuffSize = numPointsToDraw;
    self->pointBuffer = (double *) realloc(self->pointBuffer, numPointsToDraw * 3 * sizeof(double));
    self->colorBuffer = (float *) realloc(self->colorBuffer, numPointsToDraw * 4 * sizeof(float));
  }

  int numLoadedPoints = 0;
  //load points into the vertex buffers
  for (int chan_idx = 0; chan_idx < self->channels->len; chan_idx++) {
    laser_channel *lchan = g_ptr_array_index(self->channels, chan_idx);
    if (!lchan->enabled)
      continue;
    //        printf("%s has %d points.\n", lchan->name, lchan->scans->size);

    int scan_count = MIN(self->param_scan_memory, bot_ptr_circular_size(lchan->scans));
    for (int scan_idx = 0; scan_idx < scan_count; scan_idx++) {
      laser_projected_scan *lscan = bot_ptr_circular_index(lchan->scans, scan_idx);

      for (int i = 0; i < lscan->npoints; i++) {
        if (lscan->point_status[i]>laser_valid_projection || lscan->points[i].z < self->param_min_draw_z|| 
               lscan->points[i].z > self->param_max_draw_z || lscan->rawScan->ranges[i] > self->param_max_draw_range ){
          continue;
        }
        else {
          float * colorV = self->colorBuffer + (4 * numLoadedPoints); //pointer into the color buffer
          colorV[3] = self->param_alpha;
          switch (color_mode) {
          case COLOR_MODE_Z:
            {
              double z;
              if (z_relative)
                z = lscan->points[i].z - lscan->origin.trans_vec[2];
              else
                z = lscan->points[i].z;

              double z_norm = (z - self->param_color_mode_z_min_z) * z_norm_scale;
              float * jetC = bot_color_util_jet(z_norm);
              memcpy(colorV, jetC, 3 * sizeof(float));
            }
            break;
          case COLOR_MODE_WORKSPACE:
            {
              double range_mapping = (lscan->rawScan->ranges[i] -0.5 /1.5); // map 0.5->2 range to be 0->1
              double z_norm=0;
              if (range_mapping >1.0){
                range_mapping = 1.0;
              }else if(range_mapping < 0.0){
                range_mapping = 0.0;
              }
              float * jetC = bot_color_util_jet(range_mapping);
              memcpy(colorV, jetC, 3 * sizeof(float));
            }
            break;
          case COLOR_MODE_INTENSITY:
            {
              if (lscan->rawScan->nintensities == lscan->rawScan->nranges) {
                double v = lscan->rawScan->intensities[i];
                if (v > 1) {
                  // SICK intensity ranges: (rarely used now)
                  // double minv = 7000, maxv = 12000; 
                  // Hokuyo Typical Ranges:
                  double minv = 300, maxv = 4000;
                  
                  v = fmax(300.0 , fmin(v,4000.0) );
                  v = (v - minv) / (maxv - minv);
                }
                float * jetC = bot_color_util_jet(v);
                memcpy(colorV, jetC, 3 * sizeof(float));
                break;
              }
            }
            // no intensity data? fall through!
          case COLOR_MODE_LASER:
            memcpy(colorV, lchan->color, 3 * sizeof(float));
            break;
          case COLOR_MODE_DRAB:/* COLOR_MODE_DRAB */
          default:
            {
              colorV[0] = 0.3;
              colorV[1] = 0.3;
              colorV[2] = 0.3;
            }
            break;
          }
          double * pointP = self->pointBuffer + (3 * numLoadedPoints);
          memcpy(pointP, point3d_as_array(&lscan->points[i]), 3 * sizeof(double));
          numLoadedPoints++;
        }
      }

    }
  }

  if (numLoadedPoints != numPointsToDraw) {
    fprintf(stderr, "ERROR: numLoadedPoints=%d, numPointsToDraw=%d \n", numLoadedPoints, numPointsToDraw);
  }

  glPushAttrib(GL_DEPTH_BUFFER_BIT | GL_POINT_BIT | GL_CURRENT_BIT);
  if (self->param_z_buffer) {
    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LESS);
  }

  if (self->param_big_points)
    glPointSize(4.0f);
  else
    glPointSize(2.0f);

 if (self->param_alpha < 1) {
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  }
  else {
    glDisable(GL_BLEND);
  }

  //render using vertex buffers
  glEnableClientState(GL_VERTEX_ARRAY);
  glEnableClientState(GL_COLOR_ARRAY);

  glColorPointer(4, GL_FLOAT, 0, self->colorBuffer);
  glVertexPointer(3, GL_DOUBLE, 0, self->pointBuffer);
  //draw
  glDrawArrays(GL_POINTS, 0, numPointsToDraw);

  glDisableClientState(GL_VERTEX_ARRAY);
  glDisableClientState(GL_COLOR_ARRAY);

  if (self->param_alpha < 1)
    glDisable(GL_BLEND);
  if (self->param_z_buffer)
    glDisable(GL_DEPTH_TEST);

  glPopAttrib();
}
コード例 #7
0
ファイル: renderer_car.c プロジェクト: Patrick6289/navguide
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);
    }
}