void vehicle_state::set_controls(double steering_angle, double throttle_fraction, double brake_pressure) { if(throttle_fraction > param.max_throttle) throttle_fraction = param.max_throttle; if(brake_pressure > param.max_brake) brake_pressure = param.max_brake; if(steering_angle > dgc_d2r(param.max_steering)) steering_angle = dgc_d2r(param.max_steering); if(steering_angle < -dgc_d2r(param.max_steering)) steering_angle = -dgc_d2r(param.max_steering); throttle = throttle_fraction - brake_pressure/100.0; commanded_wheel_angle = steering_angle / param.steering_ratio; if(brake_pressure > 0) commanded_forward_accel = -brake_pressure * param.brake_decel_coef; else commanded_forward_accel = throttle_fraction * param.throttle_accel_coef; }
void vehicle_state::update(double dt) { double alpha_f, alpha_r, Fxf, Fxr, Fyf, Fyr; double x_dot, y_dot, yaw_dot; double v_x_dot, v_y_dot, yaw_rate_dot, wheel_angle_dot; double wheel_angle_rate_dot; if(shifting) { shift_timer += dt; if(shift_timer > 2.0) { direction = commanded_direction; shifting = 0; } } /* if we get a change of direction command, max brake to zero speed */ if (direction != commanded_direction) actual_forward_accel = -param.max_brake * param.brake_decel_coef; else { if(commanded_forward_accel < 0) actual_forward_accel += 1 / 0.3 * (commanded_forward_accel - actual_forward_accel) * dt; else actual_forward_accel += 1 / 0.4 * (commanded_forward_accel - actual_forward_accel) * dt; actual_forward_accel = commanded_forward_accel; // if(paused && v_x > 0) if(paused) actual_forward_accel = -3.0; // if(paused && v_x < 0) // actual_forward_accel = 3.0; } if(param.bicycle_model && v_x > 0) { /* front wheel drive car - input force */ Fxr = 0; if(direction) Fxf = param.mass * actual_forward_accel; else Fxf = param.mass * -actual_forward_accel; /* steering dynamics */ if(param.torque_mode) { wheel_angle_rate_dot = (torque - param.steering_ratio / dgc_d2r(param.max_steering_rate) * wheel_angle_rate) / param.steer_inertia; wheel_angle_dot = wheel_angle_rate; } else { wheel_angle_dot = 1 / param.tau * (commanded_wheel_angle - wheel_angle); wheel_angle_rate = wheel_angle_dot; wheel_angle_rate_dot = 0; } /* full bicycle model with limited steering dynamics */ if(v_x < dgc_mph2ms(5)) { alpha_f = atan2(v_y + yaw_rate * param.a, dgc_mph2ms(5)) - wheel_angle; alpha_r = atan2(v_y - yaw_rate * param.b, dgc_mph2ms(5)); } else { alpha_f = atan2(v_y + yaw_rate * param.a, v_x) - wheel_angle; alpha_r = atan2(v_y - yaw_rate * param.b, v_x); } Fyf = -param.tire_stiffness * alpha_f; Fyr = -param.tire_stiffness * alpha_r; x_dot = v_x * cos(yaw) - v_y * sin(yaw); y_dot = v_x * sin(yaw) + v_y * cos(yaw); yaw_dot = yaw_rate; v_x_dot = 1 / param.mass * (Fxr + Fxf * cos(wheel_angle) - Fyf * sin(wheel_angle)) + yaw_rate * v_y; v_y_dot = 1 / param.mass * (Fyr + Fxf * sin(wheel_angle) + Fyf * cos(wheel_angle)) - yaw_rate * v_x; yaw_rate_dot = 1 / param.iz * (param.a * Fxf * sin(wheel_angle) + param.a * Fyf * cos(wheel_angle) - param.b * Fyr); } else { /* steering dynamics */ if (param.torque_mode) { wheel_angle_rate_dot = (torque - param.steering_ratio / dgc_d2r(param.max_steering_rate) * wheel_angle_rate) / param.steer_inertia; wheel_angle_dot = wheel_angle_rate; //fprintf( stderr, "(%f)\n", torque ); } else { wheel_angle_dot = 1 / param.tau * (commanded_wheel_angle - wheel_angle); wheel_angle_rate = wheel_angle_dot; wheel_angle_rate_dot = 0; } x_dot = v_x * cos(yaw); y_dot = v_x * sin(yaw); yaw_dot = v_x * tan(wheel_angle) / param.wheel_base; if (!direction) { actual_forward_accel *= -1; } /* if(direction) v_x_dot = actual_forward_accel; else v_x_dot = -actual_forward_accel;*/ v_x_dot = actual_forward_accel; //fprintf( stderr, "[%f]", actual_forward_accel ); v_y_dot = 0; yaw_rate_dot = 0; } if(!my_isnormal(x)) { fprintf(stderr, "S before something wrong with pos x %f %s\n", x, why_not_normal(x)); } // state (x,y) tracks center of gravity; // if using simple bicycle model, need to convert to rear axle for update double cg_to_ra_dist = -DGC_PASSAT_IMU_TO_CG_DIST + DGC_PASSAT_IMU_TO_FA_DIST - DGC_PASSAT_WHEEL_BASE; if (!param.bicycle_model) { x += cg_to_ra_dist * cos(yaw); y += cg_to_ra_dist * sin(yaw); } /* first order integration */ x += x_dot * dt; y += y_dot * dt; yaw += yaw_dot * dt; v_x += v_x_dot * dt; if(direction && v_x < 0) { actual_forward_accel = 0; v_x = 0; } else if(!direction && v_x > 0) { actual_forward_accel = 0; v_x = 0; } v_y += v_y_dot * dt; yaw_rate += yaw_rate_dot * dt; if(v_x == 0) { v_y = 0; yaw_rate = 0; } /* convert back to cg coordinates */ if (!param.bicycle_model) { x -= cg_to_ra_dist * cos(yaw); y -= cg_to_ra_dist * sin(yaw); } if(!my_isnormal(x)) { fprintf(stderr, "S after something wrong with pos x %f %s\n", x, why_not_normal(x)); fprintf(stderr, "commanded steering %f accel %f dt %f\n", commanded_wheel_angle, commanded_forward_accel, dt); exit(0); } if(wheel_angle > dgc_d2r(param.max_steering) / param.steering_ratio) { wheel_angle = dgc_d2r(param.max_steering) / param.steering_ratio; if(wheel_angle_rate > 0) wheel_angle_rate = 0; if(wheel_angle_dot > 0) wheel_angle_dot = 0; if(wheel_angle_rate_dot > 0) wheel_angle_rate_dot = 0; } else if(wheel_angle < -dgc_d2r(param.max_steering) / param.steering_ratio) { wheel_angle = -dgc_d2r(param.max_steering) / param.steering_ratio; if(wheel_angle_rate < 0) wheel_angle_rate = 0; if(wheel_angle_dot < 0) wheel_angle_dot = 0; if(wheel_angle_rate_dot < 0) wheel_angle_rate_dot = 0; } wheel_angle += wheel_angle_dot * dt; wheel_angle_rate += wheel_angle_rate_dot * dt; if(param.bicycle_model) { lateral_accel = v_y_dot; } else { lateral_accel = dgc_square(v_x) * tan(wheel_angle) / param.wheel_base; yaw_rate = v_x * tan(wheel_angle) / param.wheel_base; } /* after coming to a stop, start timer for shift */ if(direction != commanded_direction && !shifting && v_x == 0) { shift_timer = 0; shifting = 1; } //fprintf (stderr, "\r v_x = %4.3f wheel_angle = %4.3f %4.3f %4.3f accel = %4.3f ", v_x, // commanded_wheel_angle, wheel_angle, wheel_angle_dot, v_x_dot) ; }
int dgc_transform_read(dgc_transform_t t, const char *filename) { FILE *fp; char *err, *mark, *unit, line[1001]; double arg, x, y, z; /* start with identity transform */ dgc_transform_identity(t); fp = fopen(filename, "r"); if(fp == NULL) { dgc_warning("Error: could not open transform file %s.\n", filename); return -1; } do { err = fgets(line, 1000, fp); if(err != NULL) { unit = dgc_next_word(line); mark = dgc_next_word(unit); if(strncasecmp(line, "rx ", 3) == 0) { arg = strtod(mark, &mark); if(strncasecmp(unit, "deg", 3) == 0) arg = dgc_d2r(arg); dgc_transform_rotate_x(t, arg); } else if(strncasecmp(line, "ry ", 3) == 0) { arg = strtod(mark, &mark); if(strncasecmp(unit, "deg", 3) == 0) arg = dgc_d2r(arg); dgc_transform_rotate_y(t, arg); } else if(strncasecmp(line, "rz ", 3) == 0) { arg = strtod(mark, &mark); if(strncasecmp(unit, "deg", 3) == 0) arg = dgc_d2r(arg); dgc_transform_rotate_z(t, arg); } else if(strncasecmp(line, "t ", 2) == 0) { char *a = strdup("test"); x = strtod(mark, &mark); y = strtod(mark, &mark); z = strtod(mark, &mark); if(strncasecmp(unit, "in", 2) == 0) { x *= 0.0254; y *= 0.0254; z *= 0.0254; } else if(strncasecmp(unit, "cm", 2) == 0) { x *= 0.01; y *= 0.01; z *= 0.01; } dgc_transform_translate(t, x, y, z); } else { dgc_warning("Error: could not parse line \"%s\" from %s\n", line, filename); return -1; } } } while(err != NULL); fclose(fp); return 0; }
bool Config::readCalibration(const std::string& filename) { FILE * iop; int FEnd; int linectr = 0; int n, id, enabled; int i, j; double rcf, hcf, hoff, voff, dist, distX, distY; char command[MAX_LINE_LENGTH]; char line[MAX_LINE_LENGTH]; char str1[MAX_LINE_LENGTH]; char str2[MAX_LINE_LENGTH]; char str3[MAX_LINE_LENGTH]; char str4[MAX_LINE_LENGTH]; char str5[MAX_LINE_LENGTH]; char str6[MAX_LINE_LENGTH]; char str7[MAX_LINE_LENGTH]; char str8[MAX_LINE_LENGTH]; char str9[MAX_LINE_LENGTH]; float range[64]; char *expanded_filename = NULL; min_intensity = 0; max_intensity = 255; for (i = 0; i < 64; i++) { for (j = 0; j < 256; j++) { intensity_map[i][j] = j; } } for (n = 0; n < 64; n++) { range[n] = 0.0; } range_multiplier = 1.0; spin_start = VELO_SPIN_START; expanded_filename = dgc_expand_filename(filename.c_str()); if (expanded_filename == NULL) { fprintf(stderr, "[31;1m# ERROR: could not expand filename %s[0m\n", filename.c_str()); return false; } else if ((iop = fopen(expanded_filename, "r")) == 0) { fprintf(stderr, "[31;1m# ERROR: could not open velodyne calibration file %s[0m\n", filename.c_str()); return false; } fprintf(stderr, "# INFO: read velodyne calibration file %s\n", filename.c_str()); free(expanded_filename); FEnd = 0; do { if (fgets(line, MAX_LINE_LENGTH, iop) == NULL) FEnd = 1; else { linectr++; if (sscanf(line, "%s", command) == 0) { fclose(iop); return false; } else { if (command[0] != '#') { n = sscanf(line, "%s %s %s %s %s %s %s %s %s", str1, str2, str3, str4, str5, str6, str7, str8, str9); if (n == 9) { id = atoi(str1); rcf = atof(str2); hcf = atof(str3); dist = atof(str4); distX = atof(str5); distY = atof(str6); voff = atof(str7); hoff = atof(str8); enabled = atoi(str9); if (id < 0 || id > 63) { fprintf(stderr, "[31;1m# ERROR: wrong id '%d' in line %d[0m\n", id, linectr); fclose(iop); return false; } else { rot_angle[id] = dgc_d2r(rcf); vert_angle[id] = dgc_d2r(hcf); range_offset[id] = dist; range_offsetX[id] = distX; range_offsetY[id] = distY; laser_enabled[id] = enabled; v_offset[id] = voff; h_offset[id] = hoff; } } else if (n == 2) { if (!strcasecmp(str1, "RANGE_MULTIPLIER")) { range_multiplier = atof(str2); } else if (!strcasecmp(str1, "SPIN_START")) { spin_start = atoi(str2); } else { fprintf(stderr, "[31;1m# ERROR: unknown keyword '%s' in line %d[0m\n", str1, linectr); fclose(iop); return false; } } else { fprintf(stderr, "[31;1m# ERROR: error in line %d: %s[0m\n", linectr, line); fclose(iop); return false; } } } } } while (!FEnd); fclose(iop); recomputeAngles(); findBeamOrder(); return true; }
int dgc_transform_read_string(dgc_transform_t t, char *str) { int done; char *end, *mark, *mark2, *unit; char line[1001]; double arg, x, y, z; /* start with identity transform */ dgc_transform_identity(t); if(str != NULL) { mark = str; done = 0; do { end = strchr(mark, '\n'); if(end == NULL) { strcpy(line, mark); done = 1; } else { strncpy(line, mark, end - mark + 1); line[end - mark + 1] = '\0'; mark = end + 1; } if(strlen(line) > 0) { unit = dgc_next_word(line); mark2 = dgc_next_word((char *)unit); if(strncasecmp(line, "rx ", 3) == 0) { arg = strtod(mark2, &mark2); if(strncasecmp(unit, "deg", 3) == 0) arg = dgc_d2r(arg); dgc_transform_rotate_x(t, arg); } else if(strncasecmp(line, "ry ", 3) == 0) { arg = strtod(mark2, &mark2); if(strncasecmp(unit, "deg", 3) == 0) arg = dgc_d2r(arg); dgc_transform_rotate_y(t, arg); } else if(strncasecmp(line, "rz ", 3) == 0) { arg = strtod(mark2, &mark2); if(strncasecmp(unit, "deg", 3) == 0) arg = dgc_d2r(arg); dgc_transform_rotate_z(t, arg); } else if(strncasecmp(line, "t ", 2) == 0) { x = strtod(mark2, &mark2); y = strtod(mark2, &mark2); z = strtod(mark2, &mark2); if(strncasecmp(unit, "in", 2) == 0) { x *= 0.0254; y *= 0.0254; z *= 0.0254; } else if(strncasecmp(unit, "cm", 2) == 0) { x *= 0.01; y *= 0.01; z *= 0.01; } dgc_transform_translate(t, x, y, z); } else { dgc_warning("Error: could not parse line \"%s\" from transform\n", line); return -1; } } } while(!done); } return 0; }