double
SpeedControlLogic(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config)
{
double v_scl = simulator_config->max_v;
//todo transformar esses valores em parametros no ini
double a_scl = 0.1681;
double b_scl = -0.0049;
double safety_factor = 1;
double kt = carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
double v_cmd = fabs(simulator_config->target_v);
double v_cmd_max = carmen_fmax(v_scl, (kt + simulator_config->delta_t - a_scl) / b_scl);
double k_max_scl = carmen_fmin(simulator_config->maximum_steering_command_curvature, a_scl + b_scl * v_cmd);
double kt_dt = carmen_get_curvature_from_phi(simulator_config->target_phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
if (fabs(kt_dt) >= k_max_scl)
v_cmd = fabs(safety_factor * v_cmd_max);
if(pose.v != 0)
return v_cmd * (pose.v / fabs(pose.v));
else
return 0;
}
double
compute_curvature(double phi, carmen_simulator_ackerman_config_t *simulator_config)
{
double curvature;
curvature = carmen_get_curvature_from_phi(phi, simulator_config->v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
return (curvature);
}
Robot_State
TrajectoryLookupTable::predict_next_pose(Robot_State &robot_state, Command &requested_command,
double full_time_interval, double *distance_traveled, double delta_t)
{
int n = floor(full_time_interval / delta_t);
double remaining_time = full_time_interval - ((double) n * delta_t);
Robot_State achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo
double curvature = carmen_get_curvature_from_phi(
requested_command.phi, requested_command.v,
GlobalState::robot_config.understeer_coeficient,
GlobalState::robot_config.distance_between_front_and_rear_axles);
double new_curvature = carmen_get_curvature_from_phi(
achieved_robot_state.v_and_phi.phi, achieved_robot_state.v_and_phi.v,
GlobalState::robot_config.understeer_coeficient,
GlobalState::robot_config.distance_between_front_and_rear_axles);
double max_curvature_change = GlobalState::robot_config.maximum_steering_command_rate * delta_t;
// Euler method
for (int i = 0; i < n; i++)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, &requested_command, delta_t,
new_curvature, curvature, max_curvature_change);
if (distance_traveled)
*distance_traveled += dist_walked;
}
if (remaining_time > 0.0)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, &requested_command, delta_t,
new_curvature, curvature, max_curvature_change);
if (distance_traveled)
*distance_traveled += dist_walked;
}
achieved_robot_state.pose.theta = carmen_normalize_theta(achieved_robot_state.pose.theta);
robot_state = achieved_robot_state;
return (achieved_robot_state);
}
carmen_ackerman_path_point_t
DynamicsResponse(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config)
{
double kt = carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
double kt_dt = carmen_get_curvature_from_phi(simulator_config->target_phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
double dk_dt_cmd = carmen_clamp(0 , kt_dt - kt, simulator_config->maximum_steering_command_rate); //todo descobrir qual é o comando minimo de curvatura, se existir
double dv_dt_cmd;
simulator_config->target_v = SpeedControlLogic(pose, simulator_config);
pose.phi = get_phi_from_curvature(carmen_clamp(-simulator_config->maximum_steering_command_curvature, kt + dk_dt_cmd * simulator_config->delta_t, simulator_config->maximum_steering_command_curvature), simulator_config);
if (pose.v >= 0)
dv_dt_cmd = carmen_clamp(-simulator_config->maximum_deceleration_forward, simulator_config->target_v - pose.v, simulator_config->maximum_acceleration_forward);
else
dv_dt_cmd = carmen_clamp(-simulator_config->maximum_deceleration_reverse, simulator_config->target_v - pose.v, simulator_config->maximum_acceleration_reverse);
pose.v = pose.v + dv_dt_cmd * simulator_config->delta_t;
return pose;
}
carmen_ackerman_traj_point_t
carmen_libcarmodel_recalc_pos_ackerman(carmen_ackerman_traj_point_t robot_state, double target_v, double target_phi,
double full_time_interval, double *distance_traveled, double delta_t, carmen_robot_ackerman_config_t robot_config)
{
double a = (target_v - robot_state.v) / full_time_interval;
int n = floor(full_time_interval / delta_t);
double remaining_time = full_time_interval - ((double) n * delta_t);
carmen_ackerman_traj_point_t achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo
double curvature = carmen_get_curvature_from_phi(target_phi, target_v,
robot_config.understeer_coeficient, robot_config.distance_between_front_and_rear_axles);
double new_curvature = carmen_get_curvature_from_phi(achieved_robot_state.phi, achieved_robot_state.v,
robot_config.understeer_coeficient, robot_config.distance_between_front_and_rear_axles);
double max_curvature_change = robot_config.maximum_steering_command_rate * delta_t;
// Euler method
for (int i = 0; i < n; i++)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, target_v, a, delta_t,
new_curvature, curvature, max_curvature_change, robot_config);
if (distance_traveled)
*distance_traveled += dist_walked;
}
if (remaining_time > 0.0)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, target_v, a, delta_t,
new_curvature, curvature, max_curvature_change, robot_config);
if (distance_traveled)
*distance_traveled += dist_walked;
}
achieved_robot_state.theta = carmen_normalize_theta(achieved_robot_state.theta);
robot_state = achieved_robot_state;
return (achieved_robot_state);
}
carmen_ackerman_path_point_t
motionModel(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config)
{
pose.x += pose.v * simulator_config->delta_t * cos(pose.theta);
pose.y += pose.v * simulator_config->delta_t * sin(pose.theta);
pose.theta += pose.v * simulator_config->delta_t * carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
pose.theta = carmen_normalize_theta(pose.theta);
//todo como tratar o delay?
pose = DynamicsResponse(pose, simulator_config);
return pose;
}
bool
Obstacle_Detection::is_obstacle_path2(Robot_State &robot_state, const Command &requested_command, double full_time_interval)
{
double delta_t = 0.09;
int n = floor(full_time_interval / delta_t);
double remaining_time = full_time_interval - ((double) n * delta_t);
Robot_State achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo
#ifdef OLD_STEERING_CONTROL
double curvature = carmen_get_curvature_from_phi(
requested_command.phi, requested_command.v,
GlobalState::robot_config.understeer_coeficient,
GlobalState::robot_config.distance_between_front_and_rear_axles);
double new_curvature = carmen_get_curvature_from_phi(
achieved_robot_state.v_and_phi.phi, achieved_robot_state.v_and_phi.v,
GlobalState::robot_config.understeer_coeficient,
GlobalState::robot_config.distance_between_front_and_rear_axles);
double max_curvature_change = GlobalState::robot_config.desired_steering_command_rate * delta_t;
#else
double max_phi_velocity = GlobalState::max_phi_velocity;
double max_phi_acceleration = GlobalState::max_phi_acceleration;
double phi_velocity = 0.0;
// O codigo abaixo assume phi_velocity == 0.0 no início do full_time_interval
double t_fim_descida;
double t_fim_plato;
double t_fim_subida;
Ackerman::compute_intermediate_times(t_fim_subida, t_fim_plato, t_fim_descida, max_phi_acceleration, robot_state.v_and_phi.phi, requested_command.phi,
full_time_interval, max_phi_velocity);
#endif
// Euler method
for (int i = 0; i < n; i++)
{
#ifdef OLD_STEERING_CONTROL
Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, delta_t,
new_curvature, curvature, max_curvature_change);
#else
double t = (double) i * delta_t;
Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, phi_velocity,
t, delta_t,
t_fim_descida, t_fim_plato, t_fim_subida,
max_phi_velocity, max_phi_acceleration);
#endif
if (car_border_verification(achieved_robot_state.pose))
return true;
}
if (remaining_time > 0.0)
{
#ifdef OLD_STEERING_CONTROL
Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, delta_t,
new_curvature, curvature, max_curvature_change);
#else
double t = (double) n * delta_t;
Ackerman::predict_next_pose_during_main_rrt_planning_step(achieved_robot_state, requested_command, phi_velocity,
t, remaining_time,
t_fim_descida, t_fim_plato, t_fim_subida,
max_phi_velocity, max_phi_acceleration);
#endif
if (car_border_verification(achieved_robot_state.pose))
return true;
}
return false;
}