static int
pose_listener (CTrans * ctrans, ctrans_update_type_t type, void *user)
{
if (type != CTRANS_POSE_UPDATE)
return 0;
RendererCar *self = (RendererCar*) user;
ViewHandler *vhandler = self->viewer->view_handler;
lcmtypes_pose_t pose;
ctrans_local_pose (self->ctrans, &pose);
double lastpos[3] = {0,0,0};
if (gu_ptr_circular_size(self->path))
memcpy(lastpos, gu_ptr_circular_index(self->path, 0),
3 * sizeof(double));
double diff[3];
vector_subtract_3d(pose.pos, lastpos, diff);
if (vector_magnitude_3d(diff) > 2.0) {
// clear the buffer if we jump
gu_ptr_circular_clear(self->path);
}
if (vector_magnitude_3d(diff) > 0.1 ||
gu_ptr_circular_size(self->path)==0) {
double *p = (double*) calloc(3, sizeof(double));
memcpy(p, pose.pos, sizeof(double)*3);
gu_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 = config_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] = mod2pi (self->wheelpos[i]);
}
}
memcpy (&self->last_pose, &pose, sizeof (lcmtypes_pose_t));
viewer_request_redraw(self->viewer);
return 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_find_button(GtkWidget *button, RendererCar *self)
{
ViewHandler *vhandler = self->viewer->view_handler;
double eye[3];
double lookat[3];
double up[3];
vhandler->get_eye_look(vhandler, eye, lookat, up);
double diff[3];
vector_subtract_3d(eye, lookat, diff);
double pos[3] = { 0, 0, 0 };
ctrans_local_pos (self->ctrans, pos);
vector_add_3d(pos, diff, eye);
vhandler->set_look_at(vhandler, eye, pos, up);
viewer_request_redraw(self->viewer);
}
