static void on_param_widget_changed(BotGtkParamWidget *pw, const char *name, void *user)
{
RendererLaser *self = (RendererLaser*) user;
if (!self->viewer)
return;
self->param_scan_memory = bot_gtk_param_widget_get_int(self->pw, PARAM_SCAN_MEMORY);
self->param_color_mode = bot_gtk_param_widget_get_enum(self->pw, PARAM_COLOR_MODE);
self->param_z_buffer = bot_gtk_param_widget_get_bool(self->pw, PARAM_Z_BUFFER);
self->param_big_points = bot_gtk_param_widget_get_bool(self->pw, PARAM_BIG_POINTS);
self->param_spacial_decimate = bot_gtk_param_widget_get_bool(self->pw, PARAM_SPATIAL_DECIMATE);
self->z_relative = bot_gtk_param_widget_get_bool(self->pw, PARAM_Z_RELATIVE);
self->param_color_mode_z_max_z = bot_gtk_param_widget_get_double(self->pw, PARAM_COLOR_MODE_Z_MAX_Z);
self->param_color_mode_z_min_z = bot_gtk_param_widget_get_double(self->pw, PARAM_COLOR_MODE_Z_MIN_Z);
self->param_max_draw_z = bot_gtk_param_widget_get_double(self->pw, PARAM_MAX_DRAW_Z);
self->param_min_draw_z = bot_gtk_param_widget_get_double(self->pw, PARAM_MIN_DRAW_Z);
self->param_max_draw_range = bot_gtk_param_widget_get_double(self->pw, PARAM_MAX_DRAW_RANGE);
self->param_alpha = bot_gtk_param_widget_get_double(self->pw, PARAM_ALPHA);
self->param_max_buffer_size = bot_gtk_param_widget_get_int(self->pw, PARAM_MAX_BUFFER_SIZE);
for (int i = 0; i < self->channels->len; i++) {
laser_channel *lchan = g_ptr_array_index(self->channels, i);
lchan->enabled = bot_gtk_param_widget_get_bool(pw, lchan->name);
}
bot_viewer_request_redraw(self->viewer);
}
static gboolean
set_look_at_smooth_func (BotViewHandler *vhandler)
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) vhandler->user;
gboolean end_timeout = TRUE;
double elapsed_ms = (bot_timestamp_now() - dvh->viewpath_timer_start) * 1e-3;
double time_param = elapsed_ms / dvh->viewpath_duration_ms;
if (time_param >= 1) {
time_param = 1;
end_timeout = FALSE;
}
for (int i=0; i<3; i++) {
dvh->eye[i] = dvh->origin_eye[i] + time_param * (dvh->goal_eye[i] - dvh->origin_eye[i]);
dvh->lookat[i] = dvh->origin_lookat[i] + time_param * (dvh->goal_lookat[i] - dvh->origin_lookat[i]);
dvh->up[i] = dvh->origin_up[i] + time_param * (dvh->goal_up[i] - dvh->origin_up[i]);
}
look_at_changed(dvh);
bot_viewer_request_redraw(dvh->viewer);
return end_timeout;
}
static int mouse_scroll (BotViewer *viewer, BotEventHandler *ehandler,
const double ray_start[3], const double ray_dir[3], const GdkEventScroll *event)
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) ehandler->user;
// scrolling = zoom in and out
if ( event->direction == GDK_SCROLL_UP) {
zoom_ratio (dvh, 0.9);
} else if (event->direction == GDK_SCROLL_DOWN) {
zoom_ratio (dvh, 1.1);
}
// how far from XY plane?
double dist = (ray_start[2] - dvh->lastpos[2]) / ray_dir[2];
double dq[] = {ray_start[0] - ray_dir[0]*dist,
ray_start[1] - ray_dir[1]*dist,
ray_start[2] - ray_dir[2]*dist,
1
};
double PM_dq[3];
gluProject(dq[0], dq[1], dq[2],
dvh->model_matrix, dvh->projection_matrix, dvh->viewport,
&PM_dq[0], &PM_dq[1], &PM_dq[2]);
window_space_pan(dvh, dq, event->x - PM_dq[0], -((dvh->viewport[3] - event->y) - PM_dq[1]), 1);
bot_viewer_request_redraw(viewer);
return 1;
}
static void on_lcmgl_data (const lcm_recv_buf_t *rbuf, const char *channel,
const bot_lcmgl_data_t *_msg, void *user_data )
{
BotLcmglRenderer *self = (BotLcmglRenderer*) user_data;
lcmgl_channel_t *chan = g_hash_table_lookup(self->channels, _msg->name);
if (!chan) {
chan = (lcmgl_channel_t*) calloc(1, sizeof(lcmgl_channel_t));
chan->enabled=1;
//chan->backbuffer = g_ptr_array_new();
chan->frontbuffer = g_ptr_array_new();
g_hash_table_insert(self->channels, strdup(_msg->name), chan);
bot_gtk_param_widget_add_booleans (self->pw,
0, strdup(_msg->name), 1, NULL);
}
#if 0
int current_scene = -1;
if (chan->backbuffer->len > 0) {
bot_lcmgl_data_t *ld = g_ptr_array_index(chan->backbuffer, 0);
current_scene = ld->scene;
}
// new scene?
if (current_scene != _msg->scene) {
// free objects in foreground buffer
for (int i = 0; i < chan->frontbuffer->len; i++)
bot_lcmgl_data_t_destroy(g_ptr_array_index(chan->frontbuffer, i));
g_ptr_array_set_size(chan->frontbuffer, 0);
// swap front and back buffers
GPtrArray *tmp = chan->backbuffer;
chan->backbuffer = chan->frontbuffer;
chan->frontbuffer = tmp;
bot_viewer_request_redraw( self->viewer );
}
#endif
for (int i = 0; i < chan->frontbuffer->len; i++)
bot_lcmgl_data_t_destroy(g_ptr_array_index(chan->frontbuffer, i));
g_ptr_array_set_size (chan->frontbuffer, 0);
g_ptr_array_add(chan->frontbuffer, bot_lcmgl_data_t_copy(_msg));
bot_viewer_request_redraw( self->viewer );
}
static void on_clear_button(GtkWidget *button, RendererLaser *self)
{
if (!self->viewer)
return;
for (int i = 0; i < self->channels->len; i++) {
laser_channel *lchan = g_ptr_array_index(self->channels, i);
bot_ptr_circular_clear(lchan->scans);
}
bot_viewer_request_redraw(self->viewer);
}
static void on_param_widget_changed (BotGtkParamWidget *pw, const char *name, void *user)
{
BotLcmglRenderer *self = (BotLcmglRenderer*) user;
// iterate over each channel
GList *keys = bot_g_hash_table_get_keys(self->channels);
for (GList *kiter=keys; kiter; kiter=kiter->next) {
lcmgl_channel_t *chan = g_hash_table_lookup(self->channels,
kiter->data);
chan->enabled = bot_gtk_param_widget_get_bool (pw, kiter->data);
}
g_list_free (keys);
bot_viewer_request_redraw(self->viewer);
}
static void on_clear_button(GtkWidget *button, BotLcmglRenderer *self)
{
if (!self->viewer)
return;
// iterate over each channel
GList *keys = bot_g_hash_table_get_keys(self->channels);
for (GList *kiter = keys; kiter; kiter = kiter->next) {
lcmgl_channel_t *chan = g_hash_table_lookup(self->channels, kiter->data);
// iterate over all the messages received for this channel
for (int i = 0; i < chan->frontbuffer->len; i++)
bot_lcmgl_data_t_destroy(g_ptr_array_index(chan->frontbuffer, i));
g_ptr_array_set_size(chan->frontbuffer, 0);
}
g_list_free(keys);
bot_viewer_request_redraw(self->viewer);
}
static int key_press (BotViewer *viewer, BotEventHandler *ehandler, const GdkEventKey *event)
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) ehandler->user;
int result = TRUE;
switch (event->keyval) {
case GDK_Page_Up:
zoom_ratio (dvh, 0.9);
break;
case GDK_Page_Down:
zoom_ratio (dvh, 1.1);
break;
case GDK_leftarrow:
case GDK_rightarrow:
case GDK_uparrow:
case GDK_downarrow:
default:
result = FALSE;
break;
}
bot_viewer_request_redraw(viewer);
return result;
}
static void frames_update_handler(BotFrames *bot_frames, const char *frame, const char * relative_to, int64_t utime,
void *user)
{
RendererFrames *self = (RendererFrames *) user;
BotTrans tran;
int activeSensorNum = bot_gtk_param_widget_get_enum(self->pw, PARAM_FRAME_SELECT);
if (activeSensorNum > 0) {
const char * activeSensorName = self->frameNames[activeSensorNum];
if (strcmp(frame, activeSensorName) == 0) {
const char * relative_to = bot_frames_get_relative_to(self->frames, activeSensorName);
bot_frames_get_trans(self->frames, activeSensorName, relative_to, &tran);
double rpy[3];
bot_quat_to_roll_pitch_yaw(tran.rot_quat, rpy);
bot_gtk_param_widget_set_double(self->pw, PARAM_X, tran.trans_vec[0]);
bot_gtk_param_widget_set_double(self->pw, PARAM_Y, tran.trans_vec[1]);
bot_gtk_param_widget_set_double(self->pw, PARAM_Z, tran.trans_vec[2]);
bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(rpy[0]));
bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(rpy[1]));
bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(rpy[2]));
self->updating = 0;
}
}
bot_viewer_request_redraw(self->viewer);
}
static int mouse_motion (BotViewer *viewer, BotEventHandler *ehandler,
const double ray_start[3], const double ray_dir[3], const GdkEventMotion *event)
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) ehandler->user;
double dy = event->y - dvh->last_mouse_y;
double dx = event->x - dvh->last_mouse_x;
double look[3];
bot_vector_subtract_3d (dvh->lookat, dvh->eye, look);
if (event->state & GDK_BUTTON3_MASK) {
double xy_len = bot_vector_magnitude_2d (look);
double init_elevation = atan2 (look[2], xy_len);
double left[3];
bot_vector_cross_3d (dvh->up, look, left);
bot_vector_normalize_3d (left);
double delevation = -dy * 0.005;
if (delevation + init_elevation < -M_PI/2) {
delevation = -M_PI/2 - init_elevation;
}
if (delevation + init_elevation > M_PI/8) {
delevation = M_PI/8 - init_elevation;
}
double q[4];
bot_angle_axis_to_quat(-delevation, left, q);
double newlook[3];
memcpy (newlook, look, sizeof (newlook));
bot_quat_rotate (q, newlook);
double dazimuth = -dx * 0.01;
double zaxis[] = { 0, 0, 1 };
bot_angle_axis_to_quat (dazimuth, zaxis, q);
bot_quat_rotate (q, newlook);
bot_quat_rotate (q, left);
bot_vector_subtract_3d (dvh->lookat, newlook, dvh->eye);
bot_vector_cross_3d (newlook, left, dvh->up);
look_at_changed(dvh);
}
else if (event->state & GDK_BUTTON2_MASK) {
double init_eye_dist = bot_vector_magnitude_3d (look);
double ded = pow (10, dy * 0.01);
double eye_dist = init_eye_dist * ded;
if (eye_dist > EYE_MAX_DIST)
eye_dist = EYE_MAX_DIST;
else if (eye_dist < EYE_MIN_DIST)
eye_dist = EYE_MIN_DIST;
double le[3];
memcpy (le, look, sizeof (le));
bot_vector_normalize_3d (le);
bot_vector_scale_3d (le, eye_dist);
bot_vector_subtract_3d (dvh->lookat, le, dvh->eye);
look_at_changed(dvh);
}
else if (event->state & GDK_BUTTON1_MASK) {
double dx = event->x - dvh->last_mouse_x;
double dy = event->y - dvh->last_mouse_y;
window_space_pan(dvh, dvh->manipulation_point, dx, dy, 1);
}
dvh->last_mouse_x = event->x;
dvh->last_mouse_y = event->y;
bot_viewer_request_redraw(viewer);
return 1;
}
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 on_param_widget_changed(BotGtkParamWidget *pw, const char *name, void *user)
{
RendererFrames *self = (RendererFrames*) user;
if (self->updating) {
return;
}
BotViewer *viewer = self->viewer;
int activeSensorNum = bot_gtk_param_widget_get_enum(pw, PARAM_FRAME_SELECT);
if (!strcmp(name, PARAM_FRAME_SELECT)) {
if (activeSensorNum > 0) {
self->updating = 1;
bot_viewer_set_status_bar_message(self->viewer, "Modify Calibration relative to %s", bot_frames_get_relative_to(
self->frames, self->frameNames[activeSensorNum]));
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 1);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 1);
frames_update_handler(self->frames, self->frameNames[activeSensorNum], bot_frames_get_relative_to(self->frames,
self->frameNames[activeSensorNum]), bot_timestamp_now(), self);
}
else {
bot_viewer_set_status_bar_message(self->viewer, "");
bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 0);
bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 0);
}
}
else if (!strcmp(name, PARAM_SAVE) && activeSensorNum > 0) {
char save_fname[1024];
sprintf(save_fname, "manual_calib_%s.cfg", self->frameNames[activeSensorNum]);
fprintf(stderr, "saving params to: %s\n", save_fname);
FILE * f = fopen(save_fname, "w");
double pos[3];
pos[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
pos[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
pos[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL);
rpy[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH);
rpy[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_YAW);
double rpy_rad[3];
for (int i = 0; i < 3; i++)
rpy_rad[i] = bot_to_radians(rpy[i]);
double quat[4];
bot_roll_pitch_yaw_to_quat(rpy_rad, quat);
double rod[3];
bot_quat_to_rodrigues(quat, rod);
fprintf(f, ""
"%s {\n"
"position = [%f, %f, %f];\n"
"rpy = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rpy[0], rpy[1], rpy[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fprintf(f, ""
"\n"
"%s {\n"
"position = [%f, %f, %f];\n"
"rodrigues = [%f, %f, %f];\n"
"relative_to = \"%s\";\n"
"}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rod[0], rod[1], rod[2],
bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]));
fclose(f);
bot_viewer_set_status_bar_message(self->viewer, "Calibration saved to %s", save_fname);
}
else if (activeSensorNum > 0) {
self->updating = 1;
BotTrans curr;
curr.trans_vec[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X);
curr.trans_vec[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y);
curr.trans_vec[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z);
double rpy[3];
rpy[0] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL));
rpy[1] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH));
rpy[2] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_YAW));
bot_roll_pitch_yaw_to_quat(rpy, curr.rot_quat);
if (fabs(rpy[0]) > M_PI || fabs(rpy[1]) > M_PI || fabs(rpy[2]) > M_PI) {
bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(bot_mod2pi(rpy[0])));
bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(bot_mod2pi(rpy[1])));
bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(bot_mod2pi(rpy[2])));
}
//and update the link
const char * frame_name = self->frameNames[activeSensorNum];
const char * relative_to = bot_frames_get_relative_to(self->frames, frame_name);
bot_frames_update_frame(self->frames, frame_name, relative_to, &curr, bot_timestamp_now());
}
bot_viewer_request_redraw(self->viewer);
}
static void
on_param_widget_changed (BotGtkParamWidget *pw, const char *name, void *user)
{
GfeRenderer *self = (GfeRenderer*) user;
bot_viewer_request_redraw(self->viewer);
}
