double
predict_next_pose_step(carmen_ackerman_traj_point_p new_robot_state, double target_v, double a, double delta_t,
double &achieved_curvature, const double &desired_curvature, double &max_curvature_change,
carmen_robot_ackerman_config_t robot_config)
{
carmen_ackerman_traj_point_t initial_robot_state = *new_robot_state;
double delta_curvature = fabs(achieved_curvature - desired_curvature);
double command_curvature_signal = (achieved_curvature < desired_curvature) ? 1.0 : -1.0;
achieved_curvature = achieved_curvature + command_curvature_signal * fmin(delta_curvature, max_curvature_change);
new_robot_state->phi = carmen_get_phi_from_curvature(achieved_curvature, initial_robot_state.v,
robot_config.understeer_coeficient, robot_config.distance_between_front_and_rear_axles);
new_robot_state->phi = carmen_clamp(-robot_config.max_phi, new_robot_state->phi, robot_config.max_phi);
double v0 = initial_robot_state.v;
double s = v0 * delta_t + 0.5 * a * delta_t * delta_t;
double move_x = s * cos(initial_robot_state.theta);
double move_y = s * sin(initial_robot_state.theta);
new_robot_state->x += move_x;
new_robot_state->y += move_y;
new_robot_state->theta += s * tan(new_robot_state->phi) / robot_config.distance_between_front_and_rear_axles;
new_robot_state->v = target_v;
return sqrt(move_x * move_x + move_y * move_y);
}
double
predict_next_pose_step(Robot_State *new_robot_state, Command *requested_command, double delta_t,
double &achieved_curvature, const double &desired_curvature, double &max_curvature_change)
{
Robot_State initial_robot_state = *new_robot_state;
double delta_curvature = fabs(achieved_curvature - desired_curvature);
double command_curvature_signal = (achieved_curvature < desired_curvature) ? 1.0 : -1.0;
achieved_curvature = achieved_curvature + command_curvature_signal * fmin(delta_curvature, max_curvature_change);
new_robot_state->v_and_phi.phi = carmen_get_phi_from_curvature(
achieved_curvature, initial_robot_state.v_and_phi.v,
GlobalState::robot_config.understeer_coeficient,
GlobalState::robot_config.distance_between_front_and_rear_axles);
// Tem que checar se as equacoes que governam esta mudancca de v estao corretas (precisa de um Euler?) e fazer o mesmo no caso do rrt_path_follower.
double delta_v = fabs(initial_robot_state.v_and_phi.v - requested_command->v);
double command_v_signal = (initial_robot_state.v_and_phi.v <= requested_command->v) ? 1.0 : -1.0;
new_robot_state->v_and_phi.v = initial_robot_state.v_and_phi.v + command_v_signal * delta_v;
double move_x = new_robot_state->v_and_phi.v * delta_t * cos(initial_robot_state.pose.theta);
double move_y = new_robot_state->v_and_phi.v * delta_t * sin(initial_robot_state.pose.theta);
new_robot_state->pose.x += move_x;
new_robot_state->pose.y += move_y;
new_robot_state->pose.theta += new_robot_state->v_and_phi.v * delta_t * tan(new_robot_state->v_and_phi.phi) / GlobalState::robot_config.distance_between_front_and_rear_axles;
return sqrt(move_x * move_x + move_y * move_y);
}
double
get_phi_from_curvature(double curvature, carmen_simulator_ackerman_config_t *simulator_config)
{
double phi;
phi = carmen_get_phi_from_curvature(curvature, simulator_config->v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
return (phi);
}
static double
get_motion_command_phi(double phi, double time, carmen_ackerman_traj_point_t current_robot_position, carmen_ackerman_traj_point_t center_of_the_car_front_axel, carmen_ackerman_traj_point_t point_in_trajectory)
{
double desired_phi, new_phi, v, efa;
double max_phi;
double maximum_steering_command_rate_in_degrees;
double phi_acceleration;
v = current_robot_position.v;
// See "Stanley: The Robot that Won the DARPA Grand Challenge", pp. 684,
// and "Automatic Steering Methods for Autonomous Automobile Path Tracking" CMU-RI-TR-09-08, pp. 15
// phi_gain = 1.0; // ERA 1.5 // @@@ Alberto: Tinha que ler isso do carmen ini
efa = carmen_distance_ackerman_traj(¢er_of_the_car_front_axel, &point_in_trajectory);
desired_phi = atan2(phi_gain * efa, v); // @@@ Alberto: Nao trata v negativo. O teste de sinal abaixo nao deveria ser antes desta conta?
if (phi_signal_negative(current_robot_position, center_of_the_car_front_axel, point_in_trajectory))
desired_phi = -desired_phi;
max_phi = carmen_get_phi_from_curvature(carmen_robot_ackerman_config.maximum_steering_command_curvature,
current_robot_position.v,
carmen_robot_ackerman_config.understeer_coeficient,
carmen_robot_ackerman_config.distance_between_front_and_rear_axles);
if (fabs(desired_phi) > max_phi)
desired_phi = (desired_phi / fabs(desired_phi)) * max_phi;
maximum_steering_command_rate_in_degrees = carmen_get_phi_from_curvature(carmen_robot_ackerman_config.maximum_steering_command_rate,
current_robot_position.v,
carmen_robot_ackerman_config.understeer_coeficient,
carmen_robot_ackerman_config.distance_between_front_and_rear_axles);
phi_acceleration = (desired_phi - phi) / time; // @@@ estou supondo que phi esta sempre abaixo do maximo...
if (fabs(phi_acceleration) > maximum_steering_command_rate_in_degrees) // @@@ tem que tratar o sinal de acceleration (quande de uma re)
phi_acceleration = maximum_steering_command_rate_in_degrees * (phi_acceleration / fabs(phi_acceleration));
new_phi = phi + phi_acceleration * time;
return (new_phi);
}
