oracle_explore_exploit.h

Go to the documentation of this file.

/*
 * This file is part of the CoverageControl library
 *
 * Author: Saurav Agarwal
 * Contact: sauravag@seas.upenn.edu, agr.saurav1@gmail.com
 * Repository: https://github.com/KumarRobotics/CoverageControl
 *
 * Copyright (c) 2024, Saurav Agarwal
 *
 * The CoverageControl library is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * The CoverageControl library is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * CoverageControl library. If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_ORACLE_EXPLORE_EXPLOIT_H_
#define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_ORACLE_EXPLORE_EXPLOIT_H_
 
#include <omp.h>
 
#include <fstream>
#include <iostream>
#include <random>
#include <vector>
 
#include "CoverageControl/algorithms/abstract_controller.h"
#include "CoverageControl/algorithms/near_optimal_cvt_algorithm.h"
#include "CoverageControl/coverage_system.h"
#include "CoverageControl/extern/lsap/Hungarian.h"
#include "CoverageControl/parameters.h"
#include "CoverageControl/typedefs.h"
 
namespace CoverageControl {
 
class OracleExploreExploit : public AbstractController {
 private:
  Parameters const params_;
  size_t num_robots_ = 0;
  CoverageSystem &env_;
  Voronoi voronoi_;
  std::vector<VoronoiCell> voronoi_cells_;
  PointVector robot_global_positions_;
  PointVector goals_, actions_;
  std::vector<std::vector<double>> cost_matrix_;
  MapType oracle_map_;
  MapType exploration_map_;  // Binary map: true for unexplored locations
  MapType explored_idf_map_;
  double exploration_ratio_ = 1;
  int recompute_goal_steps_ = 0, recompute_goal_counter_ = 0;
  bool is_converged_ = false;
  bool start_exploit_ = false;
 
 public:
  OracleExploreExploit(Parameters const &params, size_t const &num_robots,
                       CoverageSystem &env)
      : params_{params}, num_robots_{num_robots}, env_{env} {
    cost_matrix_.resize(num_robots_, std::vector<double>(num_robots_));
    voronoi_cells_.resize(num_robots_);
 
    robot_global_positions_ = env_.GetRobotPositions();
    actions_.resize(num_robots_);
    goals_.resize(num_robots_);
 
    // The oracle map is designed to store the pixels seen by any robot
    oracle_map_ =
        MapType::Constant(params_.pWorldMapSize, params_.pWorldMapSize, 0);
    exploration_map_ =
        MapType::Constant(params_.pWorldMapSize, params_.pWorldMapSize, 1);
    explored_idf_map_ =
        MapType::Constant(params_.pWorldMapSize, params_.pWorldMapSize, 0);
    UpdateOracleMap();
    ComputeGoals();
  }
 
  PointVector GetActions() { return actions_; }
 
  MapType const &GetOracleMap() { return oracle_map_; }
 
  void SetGoals(PointVector const &goals) { goals_ = goals; }
 
  auto GetGoals() { return goals_; }
 
  std::vector<VoronoiCell> GetVoronoiCells() { return voronoi_cells_; }
 
  void UpdateOracleMap() {
    robot_global_positions_ = env_.GetRobotPositions();
    for (size_t i = 0; i < num_robots_; ++i) {
      MapUtils::MapBounds index, offset;
      MapUtils::ComputeOffsets(params_.pResolution, robot_global_positions_[i],
                               params_.pSensorSize, params_.pWorldMapSize,
                               index, offset);
      explored_idf_map_.block(index.left + offset.left,
                              index.bottom + offset.bottom, offset.width,
                              offset.height) =
          env_.GetRobotSensorView(i).block(offset.left, offset.bottom,
                                           offset.width, offset.height);
      exploration_map_.block(index.left + offset.left,
                             index.bottom + offset.bottom, offset.width,
                             offset.height) =
          MapType::Zero(params_.pSensorSize, params_.pSensorSize);
    }
    exploration_ratio_ = static_cast<double>(exploration_map_.count()) /
                         (params_.pWorldMapSize * params_.pWorldMapSize);
    oracle_map_ = explored_idf_map_ + 2 * exploration_ratio_ * exploration_map_;
  }
 
  double GetExplorationRatio() const { return exploration_ratio_; }
 
  void ComputeGoals() {
    /* voronoi_ = Voronoi(robot_global_positions_, oracle_map_,
     * params_.pWorldMapSize, */
    /*                    params_.pResolution); */
    /* for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) { */
    /*   goals_[iRobot] = voronoi_cells_[iRobot].centroid; */
    /* } */
    robot_global_positions_ = env_.GetRobotPositions();
    voronoi_ = NearOptimalCVTAlgorithm(
        params_.pLloydNumTries, params_.pLloydMaxIterations, num_robots_,
        oracle_map_, params_.pWorldMapSize, params_.pResolution);
    voronoi_cells_ = voronoi_.GetVoronoiCells();
#pragma omp parallel for num_threads(num_robots_)
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      for (size_t jCentroid = 0; jCentroid < voronoi_cells_.size();
           ++jCentroid) {
        cost_matrix_[iRobot][jCentroid] = (robot_global_positions_[iRobot] -
                                           voronoi_cells_[jCentroid].centroid())
                                              .norm();
      }
    }
    HungarianAlgorithm HungAlgo;
    std::vector<int> assignment;
    HungAlgo.Solve(cost_matrix_, assignment);
 
    goals_.resize(num_robots_);
    auto ordered_voronoi_cells = voronoi_cells_;
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      goals_[iRobot] = voronoi_cells_[assignment[iRobot]].centroid();
      ordered_voronoi_cells[iRobot] = voronoi_cells_[assignment[iRobot]];
    }
    voronoi_cells_ = ordered_voronoi_cells;
    if (exploration_ratio_ < 0.1) {
      start_exploit_ = true;
    } else {
      recompute_goal_steps_ = ceil(-12.5 * exploration_ratio_ + 22.5);
    }
  }
 
  int ComputeActions() {
    if (start_exploit_ == false) {
      if (recompute_goal_counter_ >= recompute_goal_steps_ or
          is_converged_ == true) {
        recompute_goal_counter_ = 0;
        ComputeGoals();
      }
      ++recompute_goal_counter_;
    }
    is_converged_ = not env_.StepRobotsToGoals(goals_, actions_);
    UpdateOracleMap();
    return 0;
  }
 
  bool IsConverged() { return is_converged_; }
};
 
} /* namespace CoverageControl */
#endif  // CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_ORACLE_EXPLORE_EXPLOIT_H_