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 update_gl_matrices(BotViewer *viewer, BotViewHandler *vhandler)
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) vhandler->user;
glGetIntegerv(GL_VIEWPORT, dvh->viewport);
dvh->width = dvh->viewport[2];
dvh->height = dvh->viewport[3];
dvh->aspect_ratio = ((double) dvh->width) / dvh->height;
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
if (dvh->projection_type == BOT_VIEW_ORTHOGRAPHIC) {
double le[3];
bot_vector_subtract_3d (dvh->eye, dvh->lookat, le);
double dist = bot_vector_magnitude_3d (le) *
tan (dvh->fov_degrees * M_PI / 180 / 2);
glOrtho (-dist*dvh->aspect_ratio, dist*dvh->aspect_ratio,
-dist, dist, -10*dist, EYE_MAX_DIST * 2);
} else {
gluPerspective (dvh->fov_degrees, dvh->aspect_ratio, 0.1, EYE_MAX_DIST * 2);
}
glGetDoublev(GL_PROJECTION_MATRIX, dvh->projection_matrix);
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
glMultMatrixd(dvh->model_matrix);
}
void
bot_trans_set_from_velocities(BotTrans *dest, const double angular_rate[3],
const double velocity[3], double dt)
{
//see Frazolli notes, Aircraft Stability and Control, lecture 2, page 15
if (dt == 0) {
bot_trans_set_identity(dest);
return;
}
double identity_quat[4] = { 1, 0, 0, 0 };
double norm_angular_rate = bot_vector_magnitude_3d(angular_rate);
if (norm_angular_rate == 0) {
double delta[3];
memcpy(delta, velocity, 3 * sizeof(double));
bot_vector_scale_3d(delta, dt);
bot_trans_set_from_quat_trans(dest, identity_quat, delta);
return;
}
//"exponential of a twist: R=exp(skew(omega*t));
//rescale vel, omega, t so ||omega||=1
//trans = (I-R)*(omega \cross v) + (omega \dot v) *omega* t
// term2 term3
// R*(omega \cross v)
// term1
//rescale
double t = dt * norm_angular_rate;
double omega[3], vel[3];
memcpy(omega, angular_rate, 3 * sizeof(double));
memcpy(vel, velocity, 3 * sizeof(double));
bot_vector_scale_3d(omega, 1.0/norm_angular_rate);
bot_vector_scale_3d(vel, 1.0/norm_angular_rate);
//compute R (quat in our case)
bot_angle_axis_to_quat(t, omega, dest->rot_quat);
//cross and dot products
double omega_cross_vel[3];
bot_vector_cross_3d(omega, vel, omega_cross_vel);
double omega_dot_vel = bot_vector_dot_3d(omega, vel);
double term1[3];
double term2[3];
double term3[3];
//(I-R)*(omega \cross v) = term2
memcpy(term1, omega_cross_vel, 3 * sizeof(double));
bot_quat_rotate(dest->rot_quat, term1);
bot_vector_subtract_3d(omega_cross_vel, term1, term2);
//(omega \dot v) *omega* t
memcpy(term3, omega, 3 * sizeof(double));
bot_vector_scale_3d(term3, omega_dot_vel * t);
bot_vector_add_3d(term2, term3, dest->trans_vec);
}
static void zoom_ratio(BotDefaultViewHandler *dvh, double ratio)
{
double le[3];
bot_vector_subtract_3d (dvh->eye, dvh->lookat, le);
double eye_dist = bot_vector_magnitude_3d (le);
eye_dist *= ratio;
if (eye_dist < EYE_MIN_DIST) return;
if (eye_dist > EYE_MAX_DIST) return;
bot_vector_normalize_3d (le);
bot_vector_scale_3d (le, eye_dist);
bot_vector_add_3d (le, dvh->lookat, dvh->eye);
look_at_changed(dvh);
}
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;
}
/** Given a coordinate in scene coordinates dq, modify the camera
such that the screen projection of point dq moves (x,y) pixels.
**/
static void window_space_pan(BotDefaultViewHandler *dvh, double dq[], double x, double y, int preserveZ)
{
double orig_eye[3], orig_lookat[3];
memcpy(orig_eye, dvh->eye, 3 * sizeof(double));
memcpy(orig_lookat, dvh->lookat, 3 * sizeof(double));
y *= -1; // y is upside down...
double left[3], up[3];
// compute left and up vectors
if (!preserveZ) {
// constraint translation such that the distance between the
// eye and the lookat does not change; i.e., that the motion
// is upwards and leftwards only.
double look_vector[3];
bot_vector_subtract_3d(dvh->lookat, dvh->eye, look_vector);
bot_vector_normalize_3d(look_vector);
bot_vector_cross_3d(dvh->up, look_vector, left);
memcpy(up, dvh->up, 3 * sizeof(double));
} else {
// abuse the left and up vectors: change them to xhat, yhat... this ensures
// that pan does not affect the camera Z height.
left[0] = 1;
left[1] = 0;
left[2] = 0;
up[0] = 0;
up[1] = 1;
up[2] = 0;
}
double A[4];
double B[2] = { x, y };
build_pan_jacobian(dvh, dq, up, left, A);
double detOriginal = A[0]*A[3] - A[1]*A[2];
// Ax = b, where x = how much we should move in the up and left directions.
double Ainverse[4] = { 0, 0, 0, 0 };
bot_matrix_inverse_2x2d(A, Ainverse);
double sol[2];
bot_matrix_vector_multiply_2x2_2d(Ainverse, B, sol);
double motionup[3], motionleft[3], motion[3];
memcpy(motionup, up, 3 * sizeof(double));
bot_vector_scale_3d(motionup, sol[0]);
memcpy(motionleft, left, 3 * sizeof(double));
bot_vector_scale_3d(motionleft, sol[1]);
bot_vector_add_3d(motionup, motionleft, motion);
double magnitude = bot_vector_magnitude_3d(motion);
double new_magnitude = fmax(fmin(magnitude,MAX_MOTION_MAGNITUDE),MIN_MOTION_MAGNITUDE);
//bot_vector_normalize_3d(motion); // if magnitude is zero it will return nan's
bot_vector_scale_3d(motion,new_magnitude/fmax(magnitude,MIN_MOTION_MAGNITUDE));
double neweye[3], newlookat[3];
bot_vector_subtract_3d(dvh->eye, motion, neweye);
bot_vector_subtract_3d(dvh->lookat, motion, newlookat);
memcpy(dvh->eye, neweye, sizeof(double)*3);
memcpy(dvh->lookat, newlookat, sizeof(double)*3);
look_at_changed(dvh);
build_pan_jacobian(dvh, dq, up, left, A);
// if the projection is getting sketchy, and it's getting worse,
// then just reject it. this is better than letting the
// projection become singular! (This only happens with preserveZ?)
double detNew = A[0]*A[3] - A[1]*A[2];
//printf(" %15f %15f\n", detOriginal, detNew);
//if (fabs(detNew) < 0.01 && fabs(detNew) <= fabs(detOriginal)) {
if ((fabs(detNew) < 25 )||(fabs(detOriginal) < 25 )) {
memcpy(dvh->eye, orig_eye, 3 * sizeof(double));
memcpy(dvh->lookat, orig_lookat, 3 * sizeof(double));
look_at_changed(dvh);
printf("skipping pan: %15f %15f\n", detOriginal, detNew);
}
}
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;
}
