oracle_bang_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_BANG_EXPLORE_EXPLOIT_H_
#define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_ORACLE_BANG_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/parameters.h"
#include "CoverageControl/typedefs.h"
 
namespace CoverageControl {
 
class OracleBangExploreExploit : 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_;
  MapType const &exploration_map_;  // Binary map: true for unexplored locations
  MapType const &explored_idf_map_;
 
  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::vector<int> random_goal_counter_;
  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;
 
 public:
  OracleBangExploreExploit(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()} {
    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);
    random_goal_counter_.resize(num_robots_, 10);
 
    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<>(-time_step_dist_, time_step_dist_);
    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) {
    best_goal = pos;
    double best_bcr = 0, best_benefit = 0;
    double pos_x_lim = std::min(pos[0] + time_step_dist_,
                                static_cast<double>(params_.pWorldMapSize));
    double pos_y_lim = std::min(pos[1] + time_step_dist_,
                                static_cast<double>(params_.pWorldMapSize));
    for (double pos_x = std::max(pos[0] - time_step_dist_, eps);
         pos_x < pos_x_lim; pos_x += 1 * params_.pResolution) {
      for (double pos_y = std::max(pos[1] - 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;
        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;
  }
 
  double GetFrontierGoal1(Point2 const &pos, Point2 &best_goal) {
    best_goal = pos;
    double best_angle = 0;
    double best_bcr = 0, best_benefit = 0, best_idf_benefit = 0;
    for (double rad = time_step_dist_; rad <= 20 * time_step_dist_;
         rad += time_step_dist_) {
      for (double angle = 0; angle < 2 * M_PI; angle += M_PI / 180) {
        Point2 qpos = pos + rad * Point2{std::cos(angle), std::sin(angle)};
        std::cout << "rad: " << rad << " " << angle << std::endl;
        qpos[0] = std::min(qpos[0], static_cast<double>(params_.pWorldMapSize));
        qpos[1] = std::min(qpos[1], static_cast<double>(params_.pWorldMapSize));
        qpos[0] = std::max(qpos[0], eps);
        qpos[1] = std::max(qpos[1], eps);
        std::cout << "qpos: " << qpos[0] << " " << qpos[1] << std::endl;
        double dist = (qpos - pos).norm();
        /* 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 unexplored_benefit = unexplored;
        double idf_benefit = 0;
        double idf_value =
            explored_idf_map_
                .block(index.left + offset.left, index.bottom + offset.bottom,
                       offset.width, offset.height)
                .sum();
        if (unexplored > 0.1 * sensor_area_) {
          idf_benefit = 2.5 * idf_value;
        }
        double benefit = unexplored_benefit + idf_benefit;
        double bcr = benefit / dist;
        double bcr_diff = bcr - best_bcr;
        bool update = false;
        if (bcr_diff < -better_threshold_) {
          continue;
        }
        if (std::abs(bcr_diff) < better_threshold_) {
          if (best_angle > angle) {
            update = true;
          }
        }
        if (bcr_diff > better_threshold_ or update == true) {
          best_bcr = bcr;
          best_goal = qpos;
          best_benefit = benefit;
          /* best_unexplored_benefit = unexplored_benefit; */
          best_idf_benefit = idf_benefit;
          best_angle = angle;
        }
      }
      if (best_benefit > better_threshold_) {
        break;
      }
      if (best_idf_benefit > 0.1) {
        break;
      }
    }
    /* std::cout << "Best goal: " << best_bcr << " " << best_goal[0] << " " <<
     * best_goal[1] << " " << best_unexplored_benefit << "  " <<
     * best_idf_benefit << 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) {
      if (robot_status_[iRobot] == 0) {
        double best_benefit =
            GetFrontierGoal(robot_global_positions_[iRobot], goals_[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 */
    PointVector exploit_robots_pos;
    std::vector<int> exploit_robots;
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      if (robot_status_[iRobot] != 0) {
        exploit_robots_pos.push_back(robot_global_positions_[iRobot]);
        exploit_robots.push_back(iRobot);
      }
    }
 
    Voronoi exploit_voronoi(
        exploit_robots_pos, explored_idf_map_,
        Point2(params_.pWorldMapSize, params_.pWorldMapSize),
        params_.pResolution);
    auto exploit_voronoi_cells = exploit_voronoi.GetVoronoiCells();
 
    for (size_t iCell = 0; iCell < exploit_robots.size(); ++iCell) {
      size_t iRobot = exploit_robots[iCell];
      auto goal = exploit_voronoi_cells[iCell].centroid();
      goals_[iRobot] = goal;
    }
 
    int num_exploring_robots = num_robots_ - exploit_robots.size();
    std::cout << "Number of exploring_robots: " << num_exploring_robots
              << std::endl;
    std::cout << "Exploration ratio: " << env_.GetExplorationRatio() * 100
              << std::endl;
 
    /* if (num_exploring_robots == 0) { */
    /*   goals_ = LloydOffline(params_.pLloydNumTries,
     * params_.pLloydMaxIterations, */
    /*                         num_robots_, explored_idf_map_,
     * params_.pWorldMapSize, */
    /*                         params_.pResolution, robot_global_positions_); */
    /*   trigger_exploit_ = true; */
    /* } */
  }
 
  int ComputeActions() {
    if (first_step_ == true) {
      first_step_ = false;
      robot_global_positions_ = env_.GetRobotPositions();
      for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
        goals_[iRobot] = robot_global_positions_[iRobot] +
                         Point2{rand_dist_(gen_), rand_dist_(gen_)};
      }
 
    } else if (trigger_exploit_ == false) {
      ComputeGoals();
    }
    /* env_.StepRobotsToGoals(goals_, actions_); */
    is_converged_ = true;
    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 (actions_[iRobot].norm() > eps) {
        is_converged_ = false;
      }
    }
    return 0;
  }
 
  bool IsConverged() { return is_converged_; }
};
 
} /* namespace CoverageControl */
#endif  // CPPSRC_CORE_INCLUDE_COVERAGECONTROL_ALGORITHMS_ORACLE_BANG_EXPLORE_EXPLOIT_H_