예제 #1
0
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;
}
예제 #2
0
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;
}