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_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_clear_button(GtkWidget *button, RendererCar *self)
{
bot_ptr_circular_clear(self->path);
viewer_request_redraw(self->viewer);
}
