Пример #1
0
/**
 * All of the area coverage algorithms just check if they have reached close
 * enough to their destination. If so, they generate a new destination.
 */
int
gams::algorithms::area_coverage::BaseAreaCoverage::plan(void)
{
  if (platform_ && *platform_->get_platform_status()->movement_available)
  {
    pose::Position loc = platform_->get_location();
    pose::Position next_loc(platform_->get_frame(),
      next_position_.longitude(), next_position_.latitude(),
      next_position_.altitude());

    double dist = loc.distance_to(next_loc);

    madara_logger_ptr_log(gams::loggers::global_logger.get(),
      gams::loggers::LOG_DETAILED,
      "gams::algorithms::area_coverage::BaseAreaCoverage::plan:" \
      " distance between points is %f(need %f accuracy)\n",
      dist, platform_->get_accuracy());

    if (loc.approximately_equal(next_loc, platform_->get_accuracy()))
    {
      madara_logger_ptr_log(gams::loggers::global_logger.get(),
        gams::loggers::LOG_DETAILED,
        "gams::algorithms::area_coverage::BaseAreaCoverage::plan:" \
        " generating new position\n");
      generate_new_position();
    }
  }
  return 0;
}
Пример #2
0
int
gams::algorithms::Move::analyze (void)
{
  int result (OK);

  if (platform_ && *platform_->get_platform_status ()->movement_available)
  {
    if (status_.finished.is_false ())
    {
      if (move_index_ < locations_.size ())
      {
        madara_logger_ptr_log (gams::loggers::global_logger.get (),
          gams::loggers::LOG_MAJOR,
          "Move::analyze:" \
          " currently moving to location %d -> [%s].\n",
          (int)move_index_,
          locations_[move_index_].to_string ().c_str ());

        // check our distance to the next location
        utility::Location loc = platform_->get_location ();
        utility::Location next_loc (platform_->get_frame (),
          locations_[move_index_].y, locations_[move_index_].x,
          locations_[move_index_].z);

        madara_logger_ptr_log (gams::loggers::global_logger.get (),
          gams::loggers::LOG_DETAILED,
          "Move::analyze:" \
          " distance between points is %f (need %f accuracy)\n",
          loc.distance_to (next_loc), platform_->get_accuracy ());

        if (loc.approximately_equal (next_loc, platform_->get_accuracy ()))
        {
          // if we are at the end of the location list
          if (move_index_ == locations_.size () - 1)
          {
            ++cycles_;

            madara_logger_ptr_log (gams::loggers::global_logger.get (),
              gams::loggers::LOG_MAJOR,
              "Move::analyze:" \
              " finished waypoints cycle number %d of %d target cycles\n",
              cycles_, repeat_);

            if (cycles_ >= repeat_ && repeat_ >= 0)
            {
              madara_logger_ptr_log (gams::loggers::global_logger.get (),
                gams::loggers::LOG_MAJOR,
                "Move::analyze:" \
                " algorithm is finished after %d cycles\n",
                cycles_);

              result |= FINISHED;
              status_.finished = 1;
            } // end if finished
            else
            {
              // reset the move index if we are supposed to cycle
              move_index_ = 0;

              madara_logger_ptr_log (gams::loggers::global_logger.get (),
                gams::loggers::LOG_MAJOR,
                "Move::analyze:" \
                " proceeding to location 0 in next cycle.\n");

            } // end if not finished
          } // end if move_index_ == end of locations
          else
          {
            // go to the next location
            ++move_index_;

            madara_logger_ptr_log (gams::loggers::global_logger.get (),
              gams::loggers::LOG_MAJOR,
              "Move::analyze:" \
              " proceeding to location %d at [%s].\n",
              (int)move_index_,
              locations_[move_index_].to_string ().c_str ());

          } // if not the end of the list
        } // end if location is approximately equal to next location
      } // end next move is within the locations list
      else
      {
        madara_logger_ptr_log (gams::loggers::global_logger.get (),
          gams::loggers::LOG_ERROR,
          "Move::analyze:" \
          " ERROR: move_index_ is greater than possible locations.\n");

        result |= FINISHED;

        status_.finished.modify ();
      } // end is not in the locations list
    } // end is not finished
    else
    {
      madara_logger_ptr_log (gams::loggers::global_logger.get (),
        gams::loggers::LOG_MINOR,
        "Move::analyze:" \
        " Algorithm has finished previously. Rebroadcasting status.finished.\n");

      result |= FINISHED;

      status_.finished.modify ();
    } // end is finished
  }
  else
  {
    madara_logger_ptr_log (gams::loggers::global_logger.get (),
      gams::loggers::LOG_MAJOR,
      "Move:analyze:" \
      " platform has not set movement_available to 1.\n");
  }

  return result;
}