示例#1
0
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;
}
示例#2
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);
}
示例#3
0
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);
}