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