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_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); }