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);
}
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;
}
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;
}
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);
}
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;
}
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();
}
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);
}
}