static void ChangeWaypoint(EControl detection_result)
{
int obs = _obstacle_waypoint;
if (obs != -1){
std::cout << "====got obstacle waypoint====" << std::endl;
std::cout << "=============================" << std::endl;
}
if (detection_result == STOP){ // STOP for obstacle
// stop_waypoint is about _others_distance meter away from obstacles
int stop_waypoint = obs - ((int)(_others_distance / _path_change.getInterval()));
std::cout << "stop_waypoint: " << stop_waypoint << std::endl;
// change waypoints to stop by the stop_waypoint
_path_change.changeWaypoints(stop_waypoint);
_path_change.avoidSuddenBraking();
_path_change.setTemporalWaypoints();
_temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
} else if (detection_result == DECELERATE) { // DECELERATE for obstacles
_path_change.setPath(_path_dk.getCurrentWaypoints());
_path_change.setDeceleration();
_path_change.setTemporalWaypoints();
_temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
} else { // ACELERATE or KEEP
_path_change.setPath(_path_dk.getCurrentWaypoints());
_path_change.avoidSuddenAceleration();
_path_change.avoidSuddenBraking();
_path_change.setTemporalWaypoints();
_temporal_waypoints_pub.publish(_path_change.getTemporalWaypoints());
}
return;
}
void PathVset::changeWaypoints(int stop_waypoint)
{
int i = 0;
int close_waypoint_threshold = 4;
int fill_in_zero = 20;
double changed_vel;
double interval = getInterval();
// change waypoints to decelerate
for (int num = stop_waypoint; num > _closest_waypoint - close_waypoint_threshold; num--){
if (!checkWaypoint(num, "changeWaypoints"))
continue;
changed_vel = sqrt(2.0*_decel*(interval*i)); // sqrt(2*a*x)
//std::cout << "changed_vel[" << num << "]: " << mps2kmph(changed_vel) << " (km/h)";
//std::cout << " distance: " << (_obstacle_waypoint-num)*interval << " (m)";
//std::cout << " current_vel: " << mps2kmph(_current_vel) << std::endl;
waypoint_follower::waypoint initial_waypoint = _path_dk.getCurrentWaypoints().waypoints[num];
if (changed_vel > _velocity_limit || //
changed_vel > initial_waypoint.twist.twist.linear.x){ // avoid acceleration
//std::cout << "too large velocity!!" << std::endl;
current_waypoints_.waypoints[num].twist.twist.linear.x = initial_waypoint.twist.twist.linear.x;
} else {
current_waypoints_.waypoints[num].twist.twist.linear.x = changed_vel;
}
i++;
}
// fill in 0
for (int j = 1; j < fill_in_zero; j++){
if (!checkWaypoint(stop_waypoint+j, "changeWaypoints"))
continue;
current_waypoints_.waypoints[stop_waypoint+j].twist.twist.linear.x = 0.0;
}
std::cout << "---changed waypoints---" << std::endl;
return;
}