/**
* 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;
}
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;
}