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 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;
}
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->lcm = globals_get_lcm ();
self->config = globals_get_config ();
self->pw = BOT_GTK_PARAM_WIDGET(bot_gtk_param_widget_new());
self->atrans = globals_get_atrans ();
self->path = bot_ptr_circular_new (MAX_POSES, free_path_element, NULL);
gtk_box_pack_start(GTK_BOX(renderer->widget), GTK_WIDGET(self->pw), TRUE,
TRUE, 0);
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_FOLLOW_POS, 1, NULL);
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_FOLLOW_YAW, 0, NULL);
char * model;
char path[256];
if (bot_conf_get_str (self->config, "renderer_car.chassis_model",
&model) == 0) {
snprintf (path, sizeof (path), "%s/%s", MODELS_DIR, model);
self->chassis_model = rwx_model_create (path);
}
if (bot_conf_get_str (self->config, "renderer_car.wheel_model",
&model) == 0) {
snprintf (path, sizeof (path), "%s/%s", MODELS_DIR, model);
self->wheel_model = rwx_model_create (path);
}
if (self->chassis_model)
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_NAME_BLING, 1,
NULL);
if (self->wheel_model)
bot_gtk_param_widget_add_booleans (self->pw, 0, PARAM_NAME_WHEELS, 1,
NULL);
self->max_draw_poses = 1000;
bot_gtk_param_widget_add_int (self->pw, PARAM_MAXPOSES,
BOT_GTK_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);
botlcm_pose_t_subscribe(self->lcm, "POSE", on_pose, self);
viewer_add_renderer(viewer, &self->renderer, render_priority);
viewer_add_event_handler(viewer, &self->ehandler, render_priority);
self->footprint = pointlist2d_new (4);
fbconf_get_vehicle_footprint (self->config,
(double*)self->footprint->points);
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);
}
