simul_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_SIMUL_EXPLORE_EXPLOIT_H_
#define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_SIMUL_EXPLORE_EXPLOIT_H_
 
#include <omp.h>
 
#include <algorithm>
#include <fstream>
#include <iostream>
#include <queue>
#include <random>
#include <set>
#include <vector>
 
#include "CoverageControl/algorithms/abstract_controller.h"
#include "CoverageControl/coverage_system.h"
#include "CoverageControl/extern/lsap/Hungarian.h"
#include "CoverageControl/map_utils.h"
#include "CoverageControl/parameters.h"
#include "CoverageControl/typedefs.h"
 
namespace CoverageControl {
class OracleSimulExploreExploit {
 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_;
  MapType const &exploration_map_;  // Binary map: true for unexplored locations
  MapType const &explored_idf_map_;
 
  std::vector<queue_t> frontiers_;
  size_t num_frontiers_ = 1;
 
  int recompute_goal_steps_ = 0, recompute_goal_counter_ = 0;
  bool is_converged_ = false;
  bool trigger_exploit_ = false;
  bool first_step_ = true;
  std::vector<int> robot_status_;  // 0: exploring, 1: exploit
  std::random_device
      rd_;  // Will be used to obtain a seed for the random number engine
  std::mt19937 gen_;
  std::uniform_real_distribution<> rand_dist_;
  double exploration_threshold_ = 0.8;
  double exploit_threshold_ = 0;
  double too_close_factor_ = 6.0;
  double sensor_area_ = 0;
  double local_win_ = 0;
  double time_step_dist_ = 0;
  double eps = 0.0001;
  double better_threshold_ = 2;
  int num_exploring_robots_ = 0;
  std::vector<int> exploit_robots_;
 
 public:
  OracleSimulExploreExploit(Parameters const &params, size_t const &num_robots,
                            CoverageSystem &env)
      : params_{params},
        num_robots_{num_robots},
        env_{env},
        exploration_map_{env.GetSystemExplorationMap()},
        explored_idf_map_{env.GetSystemExploredIDFMap()} {
    num_exploring_robots_ = num_robots_;
    voronoi_cells_.resize(num_robots_);
 
    robot_global_positions_ = env_.GetRobotPositions();
    actions_.resize(num_robots_);
    goals_ = robot_global_positions_;
    sensor_area_ = params_.pSensorSize * params_.pSensorSize;
    local_win_ = params_.pLocalMapSize / 2;
    exploit_threshold_ = 0.2 * local_win_ * local_win_;
 
    robot_status_.resize(num_robots_, 0);
    frontiers_.resize(num_robots_);
 
    std::srand(0);
    gen_ = std::mt19937(
        rd_());  // Standard mersenne_twister_engine seeded with rd_()
 
    time_step_dist_ =
        params_.pMaxRobotSpeed * params_.pTimeStep * params_.pResolution;
    rand_dist_ = std::uniform_real_distribution<>(0, 2 * M_PI);
    ComputeGoals();
  }
 
  PointVector GetActions() { return actions_; }
 
  std::vector<int> GetRobotStatus() { return robot_status_; }
 
  void SetGoals(PointVector const &goals) { goals_ = goals; }
 
  auto GetGoals() { return goals_; }
  std::vector<VoronoiCell> GetVoronoiCells() { return voronoi_cells_; }
 
  void ManageRobotStatus() {
    robot_global_positions_ = env_.GetRobotPositions();
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      robot_status_[iRobot] = 0;
 
      MapUtils::MapBounds index, offset;
      MapUtils::ComputeOffsets(params_.pResolution,
                               robot_global_positions_[iRobot], local_win_,
                               params_.pWorldMapSize, index, offset);
      double unexplored =
          exploration_map_
              .block(index.left + offset.left, index.bottom + offset.bottom,
                     offset.width, offset.height)
              .count();
      double idf_value =
          explored_idf_map_
              .block(index.left + offset.left, index.bottom + offset.bottom,
                     offset.width, offset.height)
              .sum();
 
      if (unexplored < exploit_threshold_ and idf_value > exploit_threshold_) {
        robot_status_[iRobot] = 1;
        for (size_t jRobot = 0; jRobot < iRobot; ++jRobot) {
          double goal_dist = (goals_[iRobot] - goals_[jRobot]).norm();
          if (goal_dist < too_close_factor_ * params_.pSensorSize *
                              params_.pResolution and
              env_.GetExplorationRatio() < exploration_threshold_) {
            robot_status_[jRobot] = 0;
          }
        }
      }
    }
  }
 
  double GetFrontierGoal(Point2 const &pos, Point2 &best_goal,
                         queue_t &frontiers) {
    best_goal = pos;
    double best_bcr = 0, best_benefit = 0;
    double pos_x_lim = std::min(pos[0] + 2 * time_step_dist_,
                                static_cast<double>(params_.pWorldMapSize));
    double pos_y_lim = std::min(pos[1] + 2 * time_step_dist_,
                                static_cast<double>(params_.pWorldMapSize));
    for (double pos_x = std::max(pos[0] - 2 * time_step_dist_, eps);
         pos_x < pos_x_lim; pos_x += 1 * params_.pResolution) {
      for (double pos_y = std::max(pos[1] - 2 * time_step_dist_, eps);
           pos_y < pos_y_lim; pos_y += 1 * params_.pResolution) {
        Point2 qpos{pos_x, pos_y};
        double dist = (qpos - pos).norm();
        /* if(dist < 2 * params_.pResolution) { continue; } */
        dist = std::max(dist, time_step_dist_);
        MapUtils::MapBounds index, offset;
        MapUtils::ComputeOffsets(params_.pResolution, qpos, params_.pSensorSize,
                                 params_.pWorldMapSize, index, offset);
        double unexplored =
            exploration_map_
                .block(index.left + offset.left, index.bottom + offset.bottom,
                       offset.width, offset.height)
                .count();
        double benefit = unexplored;
        if (unexplored > 0.1 * sensor_area_) {
          double idf_value =
              explored_idf_map_
                  .block(index.left + offset.left, index.bottom + offset.bottom,
                         offset.width, offset.height)
                  .count();
          benefit += 2 * idf_value;
        }
        double bcr = benefit / dist;
 
        if (frontiers.size() < num_frontiers_) {
          frontiers.push(Frontier(qpos, bcr));
        } else {
          auto worst_frontier = frontiers.top();
          if (worst_frontier.value < bcr) {
            frontiers.pop();
            frontiers.push(Frontier(qpos, bcr));
          }
        }
 
        double bcr_diff = bcr - best_bcr;
        bool update = false;
        if (bcr_diff < -kEps) {
          continue;
        }
        if (std::abs(bcr_diff) < kEps) {
          Point2 unit_best_goal = best_goal.normalized();
          Point2 unit_qpos = qpos.normalized();
          Point2 unit_curr_pos = pos.normalized();
          if (unit_best_goal.dot(unit_curr_pos) >
              unit_qpos.dot(unit_curr_pos)) {
            update = true;
          }
        }
 
        if (bcr_diff > kEps or update == true) {
          best_bcr = bcr;
          best_goal = qpos;
          best_benefit = benefit;
        }
      }
    }
    /* std::cout << "Best goal: " << best_bcr << " " << best_goal[0] << " " <<
     * best_goal[1] << std::endl; */
    return best_benefit;
  }
 
  void ComputeGoals() {
    /* Exploration Robots */
    ManageRobotStatus();
 
#pragma omp parallel for num_threads(num_robots_)
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      frontiers_[iRobot] = queue_t();
      if (robot_status_[iRobot] == 0) {
        double best_benefit =
            GetFrontierGoal(robot_global_positions_[iRobot], goals_[iRobot],
                            frontiers_[iRobot]);
        if (best_benefit < better_threshold_) {
          robot_status_[iRobot] = 1;
        }
        /* std::cout << iRobot << " " << robot_status_[iRobot] << " " <<
         * (goals_[iRobot] - robot_global_positions_[iRobot]).norm() << " " <<
         * goals_[iRobot][0] << " " << goals_[iRobot][1] << std::endl; */
      }
    }
 
    /* Exploiting Robots */
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      if (robot_status_[iRobot] != 0) {
        exploit_robots_.push_back(iRobot);
      }
    }
#pragma omp parallel for num_threads(num_robots_)
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      if (robot_status_[iRobot] != 0) {  // 1 is for exploit
        Point2 const &pos = robot_global_positions_[iRobot];
        MapUtils::MapBounds index, offset;
        MapUtils::ComputeOffsets(params_.pResolution, pos,
                                 params_.pLocalMapSize, params_.pWorldMapSize,
                                 index, offset);
 
        MapType robot_local_map = explored_idf_map_.block(
            index.left + offset.left, index.bottom + offset.bottom,
            offset.width, offset.height);
 
        /* Point2 map_translation(params_.pLocalMapSize *
         * params_.pResolution/2., params_.pLocalMapSize *
         * params_.pResolution/2.); */
        Point2 map_translation(
            (index.left + offset.left) * params_.pResolution,
            (index.bottom + offset.bottom) * params_.pResolution);
        /* map_translation -= Point2(offset.left, offset.bottom) *
         * params_.pResolution; */
 
        PointVector robot_neighbors_pos;
        for (size_t jRobot = 0; jRobot < num_robots_; ++jRobot) {
          if (iRobot == jRobot or robot_status_[jRobot] == 0) {
            continue;
          }
          Point2 relative_pos =
              robot_global_positions_[jRobot] - robot_global_positions_[iRobot];
          if (relative_pos.norm() < params_.pCommunicationRange) {
            robot_neighbors_pos.push_back(robot_global_positions_[jRobot]);
          }
        }
        PointVector robot_positions(robot_neighbors_pos.size() + 1);
 
        robot_positions[0] = robot_global_positions_[iRobot] - map_translation;
        /* std::cout << "Voronoi: " << robot_positions[0][0] << " " <<
         * robot_positions[0][1] << std::endl; */
        int count = 1;
        for (Point2 const &pos : robot_neighbors_pos) {
          robot_positions[count] = pos - map_translation;
          ++count;
        }
        Point2 map_size(offset.width, offset.height);
        Voronoi voronoi(robot_positions, robot_local_map, map_size,
                        params_.pResolution, true, 0);
        auto vcell = voronoi.GetVoronoiCell();
        goals_[iRobot] =
            vcell.centroid() + robot_positions[0] + map_translation;
        if (goals_[iRobot][0] < 0 or
            goals_[iRobot][0] > params_.pWorldMapSize or
            goals_[iRobot][1] < 0 or
            goals_[iRobot][1] > params_.pWorldMapSize) {
          std::cout << "Goal out of bounds: " << goals_[iRobot][0] << " "
                    << goals_[iRobot][1] << std::endl;
          std::cout << robot_global_positions_[iRobot][0] << " "
                    << robot_global_positions_[iRobot][1] << std::endl;
          std::cout << vcell.centroid()[0] << " " << vcell.centroid()[1]
                    << std::endl;
          std::cout << map_translation[0] << " " << map_translation[1]
                    << std::endl;
          std::cout << robot_local_map.sum() << std::endl;
          throw std::runtime_error(
              "Goal out of bounds: this should not have happened for convex "
              "environments");
        }
      }
    }
 
    /* int num_exploring_robots = num_robots_ - exploit_robots_.size(); */
    /* std::cout << "Number of exploring_robots: " << num_exploring_robots << "
     * with ratio: "<< env_.GetExplorationRatio() * 100<< std::endl; */
    /* std::cout << "Exploration ratio: " << env_.GetExplorationRatio() * 100 <<
     * std::endl; */
  }
 
  int ComputeActions() {
    robot_global_positions_ = env_.GetRobotPositions();
    goals_ = robot_global_positions_;
    if (first_step_ == true) {
      first_step_ = false;
      for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
        double ang = rand_dist_(gen_);
        goals_[iRobot] = robot_global_positions_[iRobot] +
                         time_step_dist_ * Point2{cos(ang), sin(ang)};
      }
 
    } else {
      ComputeGoals();
    }
    is_converged_ = true;
    num_exploring_robots_ = 0;
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      if (robot_status_[iRobot] == 0) {
        ++num_exploring_robots_;
      }
    }
 
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      actions_[iRobot] = Point2(0, 0);
      Point2 diff = goals_[iRobot] - robot_global_positions_[iRobot];
      double dist = diff.norm();
      double speed = dist / params_.pTimeStep;
      speed = std::min(params_.pMaxRobotSpeed, speed);
      Point2 direction(diff);
      direction.normalize();
      actions_[iRobot] = speed * direction;
      /* printf("Step: %ld, %d, (%f, %f) \t (%f, %f)\n", iRobot,
       * robot_status_[iRobot], goals_[iRobot][0], goals_[iRobot][1],
       * actions_[iRobot][0], actions_[iRobot][1]); */
      if (dist <= kEps) {
        continue;
      }
      if (num_exploring_robots_ == 0) {
        if (env_.CheckOscillation(iRobot)) {
          continue;
        }
      }
      is_converged_ = false;
    }
    return 0;
  }
 
  bool IsConverged() { return is_converged_; }
 
  PointVector GetFrontiers(size_t const &robot_id) const {
    PointVector frontier_points;
    auto robot_frontiers = frontiers_[robot_id];
    while (!robot_frontiers.empty()) {
      auto point = robot_frontiers.top();
      frontier_points.push_back(point.pt);
      robot_frontiers.pop();
    }
    return frontier_points;
  }
 
  PointVector GetFrontiers() const {
    PointVector frontier_points;
    for (auto robot_frontiers : frontiers_) {
      while (!robot_frontiers.empty()) {
        auto point = robot_frontiers.top();
        frontier_points.push_back(point.pt);
        robot_frontiers.pop();
      }
    }
    return frontier_points;
  }
};
 
} /* namespace CoverageControl */
#endif  // CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_SIMUL_EXPLORE_EXPLOIT_H_