-
Notifications
You must be signed in to change notification settings - Fork 20
Creating Area Coverage Algorithms
Creating Algorithms (C) or Creating Algorithms (Java)
This tutorial will cover defining custom area coverage algorithm in the C++ version of GAMS. Area
Coverage algorithms in GAMS are special algortihms that are defined by a platform moving to
specific locations. The gams::algorithms::Area_Coverage::Base_Area_Coverage
class is derived from gams::algorithms::Base (henceforth
referred to as area_coverage::Base and algorithms::Base, respectively). algorithms::Base
contains three virtual functions: analyze, plan, and execute. An additional pure virtual
function, generate_new_position is declared in area_coverage::Base. By overloading these
functions, area coverage algorithms can designed for many different area coverage tasks. For this
tutorial, we will walkthrough developing a perimeter patrol algorithm. The source code for this
tutorial can be found in gams::algorithms::area_coverage::Perimeter_Patrol.
area_coverage::Base provides definitions for the analyze, plan, and execute functions
declared in algorithms::Base functions. In addition to these three functions,
generate_new_position is declared and must be defined.
The analyze function defined in area_coverage::Base is very simple. It just increments the execution counter for the algorithm. Of the area_coverage::Base functions that are defined, this is the one that is most likely to be redefined for use with virtual sensor-based coverages.
The plan function checks if the GPS_Position member variable next_position_ has been reached. If
it has, then generate_new_position is called to generate a new next_position_.
This function must be defined in new area coverage classes and should update next_position_ for
the next desired location to which the agent should move.
This function calls the platform::move function to move to next_position_.
Our area_coverage::Perimeter_Patrol class will have an agent move along the perimeter of a
specified polygon. This is basically a series of waypoints that will be repeated ad infinitum. We
can represent this in pseudocode as:
waypoints[n] = vertices of polygon move_to_position (waypoints[0]) move_to_position (waypoints[1]) move_to_position (waypoints[2]) ... move_to_position (waypoints[n-2]) move_to_position (waypoints[n-1]) move_to_position (waypoints[0]) move_to_position (waypoints[1]) ...
The next step is to convert this code to the loop based architecture.
waypoints[n] = vertices of polygon i = 0 while (1): move_to_position (waypoints[i]) i = (i + 1) % n
This pseudocode must be written in terms of an initialization (constructor) followed by iterations of the MAPE (analyze, plan, execute) loop. For initialization, we need to determine the waypoints and assign the first waypoint.
Perimeter_Patrol.cpp:67-120
gams::algorithms::area_coverage::Perimeter_Patrol::Perimeter_Patrol (
const Madara::Knowledge_Record& region_id,
Madara::Knowledge_Engine::Knowledge_Base * knowledge,
platforms::Base * platform,
variables::Sensors * sensors,
variables::Self * self) :
Base_Area_Coverage (knowledge, platform, sensors, self)
{
// initialize some status variables
status_.init_vars (*knowledge, "ppac");
// get waypoints
utility::Region reg = utility::parse_search_area (
*knowledge, region_id.to_string ()).get_convex_hull ();
vector<utility::GPS_Position> vertices = reg.points;
// find closest waypoint as starting point
size_t closest = 0;
utility::GPS_Position current;
current.from_container (self_->device.location);
double min_distance = current.distance_to (vertices[0]);
for (size_t i = 1; i < vertices.size (); ++i)
{
double dist = current.distance_to (vertices[i]);
if (min_distance > dist)
{
dist = min_distance;
closest = i;
}
}
// add some intermediate points
const size_t NUM_INTERMEDIATE_PTS = 5;
for (size_t i = 0; i < vertices.size(); ++i)
{
utility::GPS_Position start = vertices[(i + closest) % vertices.size ()];
utility::GPS_Position end = vertices[(i + closest + 1) % vertices.size ()];
double lat_dif = start.latitude () - end.latitude ();
double lon_dif = start.longitude () - end.longitude ();
for (size_t j = 0; j < NUM_INTERMEDIATE_PTS; ++j)
{
utility::GPS_Position temp (
start.latitude () - lat_dif * j / NUM_INTERMEDIATE_PTS,
start.longitude () - lon_dif * j / NUM_INTERMEDIATE_PTS,
2.0);
waypoints_.push_back (temp);
}
}
// set next_position_
cur_waypoint_ = 0;
next_position_ = waypoints_[cur_waypoint_];
}
There is nothing special to be done for analyze beyond what is done in area_coverage::Base, so we
will use that version.
Base_Area_Coverage.cpp:83-88
int
gams::algorithms::area_coverage::Base_Area_Coverage::analyze ()
{
++executions_;
return 0;
}
Plan needs to determine if we have reached the current waypoint and, if so, update the waypoint to the next one. This is already defined in area_coverage::Base.
Base_Area_Coverage.cpp:97-110
int
gams::algorithms::area_coverage::Base_Area_Coverage::plan ()
{
// generate new next position if necessary
utility::GPS_Position current;
current.from_container (self_->device.location);
if (current.approximately_equal(next_position_,
platform_->get_gps_accuracy ()))
{
generate_new_position();
}
return 0;
}
generate_new_position updates next_position_ after an agent reaches next_position_. Since
this is a preplanned algorithm, this is a simple operation.
Perimeter_Patrol.cpp:138-143
void
gams::algorithms::area_coverage::Perimeter_Patrol::generate_new_position ()
{
cur_waypoint_ = (cur_waypoint_ + 1) % waypoints_.size ();
next_position_ = waypoints_[cur_waypoint_];
}
Execute on area coverage algorithms tells moves the platform to move to next_position_.
Base_Area_Coverage.cpp:90-95
int
gams::algorithms::area_coverage::Base_Area_Coverage::execute ()
{
platform_->move(next_position_);
return 0;
}
For this algorithm, we will use the cover agent command. The first argument will be
perimeter patrol or ppac (perimeter patrol area coverage). cover is already implemented as a
command, so Base_Controller.cpp::system_analyze does
not need to be updated. We still need to update Algorithm_Factory.cpp
with the new include file and update create with
the new coverage type.
#include "gams/algorithms/area_coverage/Perimeter_Patrol.h"
Algorithm_Factory.cpp:143-148]
else if (type == "perimeter patrol" || type == "ppac")
{
if (knowledge_ && sensors_ && self_ && args.size () > 0)
result = new area_coverage::Perimeter_Patrol (args[0] /* search area id*/,
knowledge_, platform_, sensors_, self_);
}
The scripts/simulation/perimeter_patrol folder contains simulation files for the new algorithm.
#!/usr/bin/perl
use lib "$ENV{GAMS_ROOT}/scripts/simulation";
use simulation;
$gams_root = $ENV{GAMS_ROOT};
$time = 3600;
$madara_debug = 0;
$period = 0.5;
$num = 1;
$sim = "perimeter_patrol";
$area = "small";
$plants = "region.0";
simulation::run($num, $time, $period, $sim, $area, $madara_debug, $plants);
perimeter_patrol.pl stores the parameters that
will be passed to the simulation module. perimeter_patrol.pl is the file that should be executed
to start the simulation.
.vrep_port=19906;
.initial_x=10;
.initial_y=10;
device.0.command="cover";
device.0.command.size=2;
device.0.command.0="ppac";
device.0.command.1="region.0";
madara_init_0.mf is the command file for agent 0 in the simulation. V-REP connection parameters, initialization parameters, and commands are located here.
For other planned path area coverage algorithms, see Snake_Area_Coverage.cpp. For virtual sensor based coverages, see Local_Pheremone_Area_Coverage.cpp or Min_Time_Area_Coverage.cpp. For more information on simulations, see Simulation.