static int getNextWaypoint()
{
int path_size = static_cast<int>(_current_waypoints.getSize());
double lookahead_threshold = getLookAheadThreshold(0);
// if waypoints are not given, do nothing.
if (_current_waypoints.isEmpty())
{
return -1;
}
// look for the next waypoint.
for(int i = 0; i < path_size; i++)
{
//if search waypoint is the last
if (i == (path_size - 1))
{
ROS_INFO("search waypoint is the last");
return i;
}
// if there exists an effective waypoint
if (getPlaneDistance(_current_waypoints.getWaypointPosition(i), _current_pose.pose.position) > lookahead_threshold)
{
return i;
}
}
//if this program reaches here , it means we lost the waypoint!
return -1;
}
static double getCmdVelocity(int waypoint)
{
if (_param_flag == MODE_DIALOG)
{
ROS_INFO_STREAM("dialog : " << _initial_velocity << " km/h (" << kmph2mps(_initial_velocity) << " m/s )");
return kmph2mps(_initial_velocity);
}
if (_current_waypoints.isEmpty())
{
ROS_INFO_STREAM("waypoint : not loaded path");
return 0;
}
double velocity = _current_waypoints.getWaypointVelocityMPS(waypoint);
//ROS_INFO_STREAM("waypoint : " << mps2kmph(velocity) << " km/h ( " << velocity << "m/s )");
return velocity;
}