coverage_system.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_COVERAGE_SYSTEM_H_
#define CPPSRC_CORE_INCLUDE_COVERAGECONTROL_COVERAGE_SYSTEM_H_
 
#define EIGEN_NO_CUDA  // Don't use eigen's cuda facility
#include <omp.h>
 
#include <Eigen/Dense>  // Eigen is used for maps
#include <algorithm>
#include <iostream>
#include <list>
#include <mutex>
#include <random>
#include <string>
#include <utility>
#include <vector>
 
#include "CoverageControl/bivariate_normal_distribution.h"
#include "CoverageControl/constants.h"
#include "CoverageControl/map_utils.h"
#include "CoverageControl/parameters.h"
#include "CoverageControl/plotter.h"
#include "CoverageControl/robot_model.h"
#include "CoverageControl/typedefs.h"
#include "CoverageControl/voronoi.h"
#include "CoverageControl/world_idf.h"
 
namespace CoverageControl {
 
class CoverageSystem {
  Parameters const params_;          
  std::shared_ptr <WorldIDF> world_idf_ptr_; 
  size_t num_robots_ = 0;            
  std::vector<RobotModel> robots_;   
  double normalization_factor_ = 0;  
  Voronoi voronoi_;                  
  std::vector<VoronoiCell> voronoi_cells_;  
  mutable std::random_device
      rd_;                    
  mutable std::mt19937 gen_;  
  mutable std::mutex mutex_;
  std::uniform_real_distribution<>
      distrib_pts_;  
  PointVector robot_global_positions_;  
  MapType
      system_map_;  
  MapType
      exploration_map_;  
  MapType
      explored_idf_map_;  
  std::vector<std::list<Point2>>
      robot_positions_history_;   
  double exploration_ratio_ = 0;  
  double weighted_exploration_ratio_ =
      0;                         
  double total_idf_weight_ = 0;  
  std::vector<PlotterData> plotter_data_;  
  std::vector<std::vector<Point2>>
      relative_positions_neighbors_;  
  std::vector<std::vector<int>>
      neighbor_ids_;  
 
  void InitSetup();
 
  void UpdateSystemMap() {
    // This is not necessarily thread safe. Do NOT parallelize this for loop
    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) =
          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(offset.width, offset.height);
    }
    system_map_ = explored_idf_map_ - exploration_map_;
    /* exploration_ratio_ = 1.0 -
     * (double)(exploration_map_.sum())/(params_.pWorldMapSize *
     * params_.pWorldMapSize); */
    /* weighted_exploration_ratio_ =
     * (double)(explored_idf_map_.sum())/(total_idf_weight_); */
    /* std::cout << "Exploration: " << exploration_ratio_ << " Weighted: " <<
     * weighted_exploration_ratio_ << std::endl; */
    /* std::cout << "Diff: " << (exploration_map_.count() -
     * exploration_map_.sum()) << std::endl; */
  }
 
  void PostStepCommands(size_t robot_id);
 
  void PostStepCommands();
 
  void ComputeAdjacencyMatrix();
 
  void UpdateRobotPositions() {
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      robot_global_positions_[iRobot] =
          robots_[iRobot].GetGlobalCurrentPosition();
    }
  }
 
  void UpdateNeighbors();
 
 public:
 
  CoverageSystem() = delete;
 
  explicit CoverageSystem(Parameters const &params);
 
  CoverageSystem(Parameters const &params, int const num_gaussians,
                 int const num_robots);
 
  CoverageSystem(Parameters const &params, int const num_gaussians,
                 int const num_polygons, int const num_robots);
 
  CoverageSystem(Parameters const &params, WorldIDF const &world_idf,
                 std::string const &pos_file_name);
 
  CoverageSystem(Parameters const &params, WorldIDF const &world_idf,
                 std::vector<Point2> const &robot_positions);
 
  CoverageSystem(Parameters const &params,
                 std::vector<BivariateNormalDistribution> const &dists,
                 std::vector<Point2> const &robot_positions);
 
 
 
  void SetLocalRobotPositions(std::vector<Point2> const &relative_pos) {
    SetRobotPositions(relative_pos);
  }
 
  void SetLocalRobotPosition(size_t const robot_id,
                             Point2 const &relative_pos) {
    robots_[robot_id].SetRobotPosition(relative_pos);
    PostStepCommands(robot_id);
  }
 
  void SetGlobalRobotPosition(size_t const robot_id, Point2 const &global_pos) {
    robots_[robot_id].SetGlobalRobotPosition(global_pos);
    PostStepCommands(robot_id);
  }
  //
  void SetGlobalRobotPositions(PointVector const &global_positions) {
    if (global_positions.size() != num_robots_) {
      throw std::length_error{
          "The size of the positions don't match with the number of robots"};
    }
    for (size_t i = 0; i < num_robots_; ++i) {
      robots_[i].SetGlobalRobotPosition(global_positions[i]);
    }
    PostStepCommands();
  }
 
  void SetRobotPositions(std::vector<Point2> const &positions) {
    if (positions.size() != num_robots_) {
      throw std::length_error{
          "The size of the positions don't match with the number of robots"};
    }
    for (size_t i = 0; i < num_robots_; ++i) {
      robots_[i].SetRobotPosition(positions[i]);
    }
    PostStepCommands();
  }
 
  void SetWorldIDF(WorldIDF const &world_idf) {
    world_idf_ptr_.reset(new WorldIDF{world_idf});
    normalization_factor_ = world_idf_ptr_->GetNormalizationFactor();
  }
 
 
  [[nodiscard]] bool StepActions(PointVector const &actions) {
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      Point2 action = actions[iRobot];
      double speed = action.norm();
      Point2 direction = action.normalized();
      if (robots_[iRobot].StepControl(direction, speed)) {
        std::cerr << "Control incorrect\n";
        return 1;
      }
    }
    PostStepCommands();
    return 0;
  }
 
  [[nodiscard]] bool StepAction(size_t const robot_id, Point2 const action) {
    double speed = action.norm();
    Point2 direction = action.normalized();
    if (robots_[robot_id].StepControl(direction, speed)) {
      std::cerr << "Control incorrect\n";
      return 1;
    }
    PostStepCommands();
    return 0;
  }
 
  [[nodiscard]] bool StepControl(size_t robot_id, Point2 const &direction,
                                 double const speed) {
    if (robots_[robot_id].StepControl(direction, speed)) {
      std::cerr << "Control incorrect\n";
      return 1;
    }
    PostStepCommands();
    return 0;
  }
 
  Point2 AddNoise(Point2 const pt) const;
 
  bool CheckOscillation(size_t const robot_id) const {
    if (params_.pCheckOscillations == false) {
      return false;
    }
    if (robot_positions_history_[robot_id].size() < 3) {
      return false;
    }
    auto const &history = robot_positions_history_[robot_id];
    Point2 const last_pos = history.back();
    auto it_end = std::next(history.crbegin(), std::min(6, static_cast<int>(history.size()) - 1));
    bool flag = false;
    int count = 0;
    std::for_each(history.crbegin(), it_end,
                  [last_pos, &count](Point2 const &pt) {
                    if ((pt - last_pos).norm() < kLargeEps) {
                      ++count;
                    }
                  });
    if (count > 2) {
      flag = true;
    }
    return flag;
  }
 
  void CheckRobotID(size_t const id) const {
    if (id >= num_robots_) {
      throw std::out_of_range{"Robot index more than the number of robots"};
    }
  }
 
  void ComputeVoronoiCells() {
    UpdateRobotPositions();
    voronoi_ = Voronoi(robot_global_positions_, GetWorldMap(),
                       Point2(params_.pWorldMapSize, params_.pWorldMapSize),
                       params_.pResolution);
    voronoi_cells_ = voronoi_.GetVoronoiCells();
  }
 
  bool StepRobotToGoal(int const robot_id, Point2 const &goal,
                       double const speed_factor = 1);
 
  bool StepRobotsToGoals(PointVector const &goals, PointVector &actions);
 
  void ClearRobotMaps() {
    for (size_t i = 0; i < num_robots_; ++i) {
      robots_[i].ClearRobotMap();
    }
  }
 
  void ClearExploredIDF() {
    explored_idf_map_ =
        MapType::Constant(params_.pWorldMapSize, params_.pWorldMapSize, 0);
  }
 
 
  int WriteRobotPositions(std::string const &file_name) const;
  int WriteRobotPositions(std::string const &file_name,
                          PointVector const &positions) const;
  int WriteEnvironment(std::string const &pos_filename,
                       std::string const &env_filename) const;
 
  void PlotFrontiers(std::string const &, int const &,
                     PointVector const &) const;
  void PlotSystemMap(std::string const &dir_name, int const &step) const {
    std::vector<int> robot_status(num_robots_, 0);
    PlotSystemMap(dir_name, step, robot_status);
  }
  void PlotSystemMap(std::string const &) const;
  void PlotSystemMap(std::string const &, int const &,
                     std::vector<int> const &) const;
  void PlotMapVoronoi(std::string const &, int const &);
  void PlotMapVoronoi(std::string const &, int const &, Voronoi const &,
                      PointVector const &) const;
  void PlotWorldMap(std::string const &, std::string const &) const;
  void PlotWorldMapRobots(std::string const &, std::string const &) const;
  void PlotInitMap(std::string const &filename) const {
    PlotInitMap("./", filename);
  }
  void PlotInitMap(std::string const &, std::string const &) const;
  void PlotRobotLocalMap(std::string const &, int const &, int const &);
  void PlotRobotSystemMap(std::string const &, int const &, int const &);
  void PlotRobotExplorationMap(std::string const &, int const &, int const &);
  void PlotRobotSensorView(std::string const &, int const &, int const &);
  void PlotRobotObstacleMap(std::string const &, int const &, int const &);
  void PlotRobotCommunicationMaps(std::string const &, int const &, int const &,
                                  size_t const &);
 
  void RenderRecordedMap(std::string const &, std::string const &) const;
  void RecordPlotData(std::vector<int> const &, std::string const &);
  void RecordPlotData(std::vector<int> const &robot_status) {
    RecordPlotData(robot_status, "system");
  }
  void RecordPlotData(std::string const &map_name) {
    std::vector<int> robot_status(num_robots_, 0);
    RecordPlotData(robot_status, map_name);
  }
  void RecordPlotData() {
    std::vector<int> robot_status(num_robots_, 0);
    RecordPlotData(robot_status, "system");
  }
 
  //
  std::shared_ptr<const WorldIDF> GetWorldIDFPtr() const {
    return world_idf_ptr_;
  }
  const WorldIDF &GetWorldIDFObject() const { return *world_idf_ptr_; }
  const MapType &GetSystemMap() const { return system_map_; }
  const MapType &GetSystemExplorationMap() const { return exploration_map_; }
  const MapType &GetSystemExploredIDFMap() const { return explored_idf_map_; }
  MapType &GetSystemExploredIDFMapMutable() { return explored_idf_map_; }
  const MapType &GetWorldMap() const { return world_idf_ptr_->GetWorldMap(); }
  MapType &GetWorldMapMutable() { return world_idf_ptr_->GetWorldMapMutable(); }
 
  MapType &GetRobotMapMutable(size_t const id) {
    CheckRobotID(id);
    return robots_[id].GetRobotMapMutable();
  }
 
  inline auto GetNumRobots() const { return num_robots_; }
  inline auto GetNumFeatures() const { return num_robots_; }
 
  inline double GetExplorationRatio() const {
    double exploration_ratio =
        1.0 - static_cast<double>(exploration_map_.sum()) /
                  (params_.pWorldMapSize * params_.pWorldMapSize);
    return exploration_ratio;
  }
 
  inline double GetWeightedExplorationRatio() const {
    double weighted_exploration_ratio =
        static_cast<double>(explored_idf_map_.sum()) / (total_idf_weight_);
    return weighted_exploration_ratio;
  }
 
  PointVector GetRelativePositonsNeighbors(size_t const robot_id);
  std::vector<int> GetNeighborIDs(size_t const robot_id) const {
    return neighbor_ids_[robot_id];
  }
 
  PointVector GetRobotPositions(bool force_no_noise = false) {
    UpdateRobotPositions();
    if (params_.pAddNoisePositions and not force_no_noise) {
      PointVector noisy_robot_global_positions;
      for (Point2 pt : robot_global_positions_) {
        noisy_robot_global_positions.push_back(AddNoise(pt));
      }
      return noisy_robot_global_positions;
    }
    return robot_global_positions_;
  }
 
  Point2 GetRobotPosition(int const robot_id,
                          bool force_no_noise = false) const {
    Point2 robot_pos;
    robot_pos[0] = robots_[robot_id].GetGlobalCurrentPosition()[0];
    robot_pos[1] = robots_[robot_id].GetGlobalCurrentPosition()[1];
    if (params_.pAddNoisePositions and not force_no_noise) {
      return AddNoise(robot_pos);
    }
    return robot_pos;
  }
 
  const MapType &GetRobotLocalMap(size_t const id) {
    CheckRobotID(id);
    return robots_[id].GetRobotLocalMap();
  }
 
  const MapType &GetRobotMap(size_t const id) const {
    CheckRobotID(id);
    return robots_[id].GetRobotMap();
  }
 
  const MapType &GetRobotExplorationMap(size_t const id) {
    CheckRobotID(id);
    return robots_[id].GetExplorationMap();
  }
 
  const MapType &GetRobotSystemMap(size_t const id) {
    CheckRobotID(id);
    return robots_[id].GetRobotSystemMap();
  }
  const MapType &GetRobotObstacleMap(size_t const id) {
    CheckRobotID(id);
    return robots_[id].GetObstacleMap();
  }
 
  const MapType &GetRobotSensorView(size_t const id) const {
    CheckRobotID(id);
    return robots_[id].GetSensorView();
  }
 
  auto GetRobotsInCommunication(size_t const id) const {
    CheckRobotID(id);
    PointVector robot_neighbors_pos;
    for (size_t i = 0; i < num_robots_; ++i) {
      if (id == i) {
        continue;
      }
      Point2 relative_pos =
          robot_global_positions_[i] - robot_global_positions_[id];
      if (relative_pos.norm() < params_.pCommunicationRange) {
        robot_neighbors_pos.push_back(relative_pos);
      }
    }
    std::sort(
        robot_neighbors_pos.begin(), robot_neighbors_pos.end(),
        [](Point2 const &a, Point2 const &b) { return a.norm() < b.norm(); });
    return robot_neighbors_pos;
  }
 
  std::pair<MapType, MapType> GetRobotCommunicationMaps(size_t const, size_t);
 
  std::vector<MapType> GetCommunicationMaps(size_t map_size) {
    std::vector<MapType> communication_maps(2 * num_robots_);
    /* #pragma omp parallel for num_threads(num_robots_) */
    for (size_t i = 0; i < num_robots_; ++i) {
      auto comm_map = GetRobotCommunicationMaps(i, map_size);
      communication_maps[2 * i] = comm_map.first;
      communication_maps[2 * i + 1] = comm_map.second;
    }
    return communication_maps;
  }
 
  auto GetObjectiveValue() {
    ComputeVoronoiCells();
    return voronoi_.GetSumIDFSiteDistSqr();
  }
 
  auto GetRobotExplorationFeatures() {
    std::vector<std::vector<double>> features(num_robots_);
#pragma omp parallel for num_threads(num_robots_)
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      features[iRobot] = robots_[iRobot].GetExplorationFeatures();
    }
    return features;
  }
 
  auto GetRobotVoronoiFeatures() {
    std::vector<std::vector<double>> features(num_robots_);
#pragma omp parallel for num_threads(num_robots_)
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      features[iRobot] = robots_[iRobot].GetVoronoiFeatures();
    }
    return features;
  }
 
  std::vector<double> GetLocalVoronoiFeatures(int const robot_id);
 
  std::vector<std::vector<double>> GetLocalVoronoiFeatures() {
    std::vector<std::vector<double>> features(num_robots_);
#pragma omp parallel for num_threads(num_robots_)
    for (size_t iRobot = 0; iRobot < num_robots_; ++iRobot) {
      features[iRobot] = GetLocalVoronoiFeatures(iRobot);
    }
    return features;
  }
 
  auto GetVoronoiCells() { return voronoi_cells_; }
  Voronoi &GetVoronoi() { return voronoi_; }
 
  auto GetVoronoiCell(int const robot_id) { return voronoi_cells_[robot_id]; }
 
  double GetNormalizationFactor() {
    normalization_factor_ = world_idf_ptr_->GetNormalizationFactor();
    return normalization_factor_;
  }
 
};
 
} /* namespace CoverageControl */
#endif  // CPPSRC_CORE_INCLUDE_COVERAGECONTROL_COVERAGE_SYSTEM_H_