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