<commit_before>
#include <opencv2/opencv.hpp>

namespace csfm {

float LinearInterpolation(const cv::Mat &image, float y, float x) {
  int ix = int(x);
  int iy = int(y);
  if (ix < 0 || ix + 1 >= image.cols || iy < 0 || iy + 1 >= image.rows) {
    return 0.0f;
  }
  float dx = x - ix;
  float dy = y - iy;
  float im00 = image.at<unsigned char>(iy, ix);
  float im01 = image.at<unsigned char>(iy, ix + 1);
  float im10 = image.at<unsigned char>(iy + 1, ix);
  float im11 = image.at<unsigned char>(iy + 1, ix + 1);
  float im0 = (1 - dx) * im00 + dx * im01;
  float im1 = (1 - dx) * im10 + dx * im11;
  return (1 - dy) * im0 + dy * im1;
}

float NormalizedCrossCorrelation(float *x, float *y, int n) {
  float sumx = 0;
  float sumy = 0;
  for (int i = 0; i < n; ++i) {
    sumx += x[i];
    sumy += y[i];
  }
  float meanx = sumx / n;
  float meany = sumy / n;

  float sumx2 = 0;
  float sumy2 = 0;
  for (int i = 0; i < n; ++i) {
    sumx2 += (x[i] - meanx) * (x[i] - meanx);
    sumy2 += (y[i] - meany) * (y[i] - meany);
  }
  float varx =  sumx2 / n;
  float vary =  sumy2 / n;

  float correlation = 0;
  for (int i = 0; i < n; ++i) {
    correlation += (x[i] - meanx) * (y[i] - meany) / sqrt(varx * vary + 1e-10);
  }
  return correlation / n;
}

void ApplyHomography(const cv::Matx33d &H,
                     double x1, double y1,
                     double *x2, double *y2) {
  double w = H(2, 0) * x1 + H(2, 1) * y1 + H(2, 2);
  *x2 = (H(0, 0) * x1 + H(0, 1) * y1 + H(0, 2)) / w;
  *y2 = (H(1, 0) * x1 + H(1, 1) * y1 + H(1, 2)) / w;
}

cv::Matx33d PlaneInducedHomography(const cv::Matx33d K1,
                                   const cv::Matx33d R1,
                                   const cv::Matx31d t1,
                                   const cv::Matx33d K2,
                                   const cv::Matx33d R2,
                                   const cv::Matx31d t2,
                                   const cv::Matx31d v) {
  cv::Matx33d R2R1 = R2 * R1.t();
  return K2 * (R2R1 + (R2R1 * t1 - t2) * v.t()) * K1.inv();
}

float DepthOfPlaneBackprojection(double x, double y,
                                 const cv::Matx33d &K,
                                 const cv::Vec3d &plane) {
  float denom  = -(plane.t() * K.inv() * cv::Vec3d(x, y, 1))(0);
  return 1.0f / std::max(1e-6f, denom);
}

cv::Vec3f PlaneFromDepthAndNormal(float x, float y,
                                  const cv::Matx33d &K,
                                  float depth,
                                  const cv::Vec3f &normal) {
  cv::Vec3f point = depth * K.inv() * cv::Vec3d(x, y, 1);
  float denom = -normal.dot(point);
  return normal / std::max(1e-6f, denom);
}

// Random non-normalized normal pointing towards the camera.
cv::Vec3f RandomNormal() {
  return cv::Vec3f(
      -1.0f + rand() * 2.0f / RAND_MAX,
      -1.0f + rand() * 2.0f / RAND_MAX,
      -1.0f
  );
}

class DepthmapEstimator {
 public:
  DepthmapEstimator()
   : patch_size_(7)
   , min_depth_(0)
   , max_depth_(0)
   , num_depth_planes_(50)
   {}

  void AddView(const double *pK,
               const double *pR,
               const double *pt,
               const unsigned char *pimage,
               int width,
               int height) {
    Ks_.emplace_back(pK);
    Rs_.emplace_back(pR);
    ts_.emplace_back(pt);
    images_.emplace_back(cv::Mat(height, width, CV_8U, (void *)pimage).clone());
  }

  void SetDepthRange(double min_depth, double max_depth, int num_depth_planes) {
    min_depth_ = min_depth;
    max_depth_ = max_depth;
    num_depth_planes_ = num_depth_planes;
  }

  void ComputeBruteForce(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    *best_depth = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);
    *best_plane = cv::Mat(images_[0].rows, images_[0].cols, CV_32FC3, 0.0f);
    *best_score = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);

    int hpz = (patch_size_ - 1) / 2;
    for (int i = hpz; i < best_depth->rows - hpz; ++i) {
      std::cout << "i " << i << "\n";
      for (int j = hpz; j < best_depth->cols - hpz; ++j) {
        for (int d = 0; d < num_depth_planes_; ++d) {
          float depth = 1 / (1 / min_depth_ + d * (1 / max_depth_ - 1 / min_depth_) / (num_depth_planes_ - 1));
          cv::Vec3f normal(0, 0, -1);
          cv::Vec3f plane = PlaneFromDepthAndNormal(j, i, Ks_[0], depth, normal);
          float score = ComputePlaneScore(i, j, plane);
          if (score > best_score->at<float>(i, j)) {
            best_score->at<float>(i, j) = score;
            best_plane->at<cv::Vec3f>(i, j) = plane;
            best_depth->at<float>(i, j) = depth;
          }
        }
      }
    }
  }

  void ComputePatchMatch(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    *best_depth = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);
    *best_plane = cv::Mat(images_[0].rows, images_[0].cols, CV_32FC3, 0.0f);
    *best_score = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);

    for (int i = 0; i < 3; ++i) {
      std::cout << "PatchMatchForwardPass " << i << "\n";
      PatchMatchForwardPass(best_depth, best_plane, best_score);
      std::cout << "PatchMatchBackwardPass " << i << "\n";
      PatchMatchBackwardPass(best_depth, best_plane, best_score);
    }
  }

  void PatchMatchForwardPass(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    int neighbors[4][2] = {{-1, -1}, {-1, 0}, {-1, 1}, {0, -1}};
    int hpz = (patch_size_ - 1) / 2;
    for (int i = hpz; i < best_depth->rows - hpz; ++i) {
      for (int j = hpz; j < best_depth->cols - hpz; ++j) {
        PatchMatchUpdatePixel(best_depth, best_plane, best_score, i, j, neighbors);
      }
    }
  }

  void PatchMatchBackwardPass(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    int neighbors[4][2] = {{0, 1}, {1, -1}, {1, 0}, {1, 1}};
    int hpz = (patch_size_ - 1) / 2;
    for (int i = best_depth->rows - hpz - 1; i >= hpz; --i) {
      for (int j = best_depth->cols - hpz - 1; j >= hpz; --j) {
        PatchMatchUpdatePixel(best_depth, best_plane, best_score, i, j, neighbors);
      }
    }
  }

  void PatchMatchUpdatePixel(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score,
                             int i, int j,
                             int neighbors[4][2]) {
    const int num_neighbors = 4;
    const int num_random = 4;
    const int num_candidates = num_neighbors + num_random;
    cv::Vec3f candidate_planes[num_candidates];
    for (int k = 0; k < num_neighbors; ++k) {
      candidate_planes[k] = best_plane->at<cv::Vec3f>(i + neighbors[k][0], j + neighbors[k][1]);
    }
    for (int k = 0; k < num_random; ++k) {
      float depth = 1 / (1 / min_depth_ + rand() * (1 / max_depth_ - 1 / min_depth_) / RAND_MAX);
      cv::Vec3f normal = RandomNormal();
      candidate_planes[num_neighbors + k] = PlaneFromDepthAndNormal(j, i, Ks_[0], depth, normal);
    }
    for (int c = 0; c < num_candidates; ++c) {
      const cv::Vec3f &plane = candidate_planes[c];
      float score = ComputePlaneScore(i, j, plane);
      if (score > best_score->at<float>(i, j)) {
        best_score->at<float>(i, j) = score;
        best_plane->at<cv::Vec3f>(i, j) = plane;
        best_depth->at<float>(i, j) = DepthOfPlaneBackprojection(j, i, Ks_[0], plane);
      }
    }
  }

  float ComputePlaneScore(int i, int j, const cv::Vec3f &plane) {
    float best_score = -1.0f;
    for (int other = 1; other < images_.size(); ++other) {
      float score = ComputePlaneImageScore(i, j, plane, other);
      if (score > best_score) {
        best_score = score;
      }
    }
    return best_score;
  }

  float ComputePlaneImageScore(int i, int j,
                               const cv::Vec3f &plane,
                               int other) {
    cv::Matx33d H = PlaneInducedHomography(Ks_[0], Rs_[0], ts_[0],
                                           Ks_[other], Rs_[other], ts_[other],
                                           plane);
    int hpz = (patch_size_ - 1) / 2;
    float patch1[patch_size_ * patch_size_];
    float patch2[patch_size_ * patch_size_];
    int counter = 0;
    for (int u = -hpz; u <= hpz; ++u) {
      for (int v = -hpz; v <= hpz; ++v) {
        patch1[counter] = images_[0].at<unsigned char>(i + u, j + v);
        double x2, y2;
        ApplyHomography(H, j + v, i + u, &x2, &y2);
        patch2[counter] = LinearInterpolation(images_[other], y2, x2);
        counter++;
      }
    }
    return NormalizedCrossCorrelation(patch1, patch2, patch_size_ * patch_size_);
  }

 private:
  std::vector<cv::Mat> images_;
  std::vector<cv::Matx33d> Ks_;
  std::vector<cv::Matx33d> Rs_;
  std::vector<cv::Matx31d> ts_;
  int patch_size_;
  double min_depth_, max_depth_;
  int num_depth_planes_;
};

}

<commit_msg>Refactor plane testing<commit_after>
#include <opencv2/opencv.hpp>

namespace csfm {

float LinearInterpolation(const cv::Mat &image, float y, float x) {
  int ix = int(x);
  int iy = int(y);
  if (ix < 0 || ix + 1 >= image.cols || iy < 0 || iy + 1 >= image.rows) {
    return 0.0f;
  }
  float dx = x - ix;
  float dy = y - iy;
  float im00 = image.at<unsigned char>(iy, ix);
  float im01 = image.at<unsigned char>(iy, ix + 1);
  float im10 = image.at<unsigned char>(iy + 1, ix);
  float im11 = image.at<unsigned char>(iy + 1, ix + 1);
  float im0 = (1 - dx) * im00 + dx * im01;
  float im1 = (1 - dx) * im10 + dx * im11;
  return (1 - dy) * im0 + dy * im1;
}

float NormalizedCrossCorrelation(float *x, float *y, int n) {
  float sumx = 0;
  float sumy = 0;
  for (int i = 0; i < n; ++i) {
    sumx += x[i];
    sumy += y[i];
  }
  float meanx = sumx / n;
  float meany = sumy / n;

  float sumx2 = 0;
  float sumy2 = 0;
  for (int i = 0; i < n; ++i) {
    sumx2 += (x[i] - meanx) * (x[i] - meanx);
    sumy2 += (y[i] - meany) * (y[i] - meany);
  }
  float varx =  sumx2 / n;
  float vary =  sumy2 / n;

  float correlation = 0;
  for (int i = 0; i < n; ++i) {
    correlation += (x[i] - meanx) * (y[i] - meany) / sqrt(varx * vary + 1e-10);
  }
  return correlation / n;
}

void ApplyHomography(const cv::Matx33d &H,
                     double x1, double y1,
                     double *x2, double *y2) {
  double w = H(2, 0) * x1 + H(2, 1) * y1 + H(2, 2);
  *x2 = (H(0, 0) * x1 + H(0, 1) * y1 + H(0, 2)) / w;
  *y2 = (H(1, 0) * x1 + H(1, 1) * y1 + H(1, 2)) / w;
}

cv::Matx33d PlaneInducedHomography(const cv::Matx33d K1,
                                   const cv::Matx33d R1,
                                   const cv::Matx31d t1,
                                   const cv::Matx33d K2,
                                   const cv::Matx33d R2,
                                   const cv::Matx31d t2,
                                   const cv::Matx31d v) {
  cv::Matx33d R2R1 = R2 * R1.t();
  return K2 * (R2R1 + (R2R1 * t1 - t2) * v.t()) * K1.inv();
}

float DepthOfPlaneBackprojection(double x, double y,
                                 const cv::Matx33d &K,
                                 const cv::Vec3d &plane) {
  float denom  = -(plane.t() * K.inv() * cv::Vec3d(x, y, 1))(0);
  return 1.0f / std::max(1e-6f, denom);
}

cv::Vec3f PlaneFromDepthAndNormal(float x, float y,
                                  const cv::Matx33d &K,
                                  float depth,
                                  const cv::Vec3f &normal) {
  cv::Vec3f point = depth * K.inv() * cv::Vec3d(x, y, 1);
  float denom = -normal.dot(point);
  return normal / std::max(1e-6f, denom);
}

// Random non-normalized normal pointing towards the camera.
cv::Vec3f RandomNormal() {
  return cv::Vec3f(
      -1.0f + rand() * 2.0f / RAND_MAX,
      -1.0f + rand() * 2.0f / RAND_MAX,
      -1.0f
  );
}

class DepthmapEstimator {
 public:
  DepthmapEstimator()
   : patch_size_(7)
   , min_depth_(0)
   , max_depth_(0)
   , num_depth_planes_(50)
   {}

  void AddView(const double *pK,
               const double *pR,
               const double *pt,
               const unsigned char *pimage,
               int width,
               int height) {
    Ks_.emplace_back(pK);
    Rs_.emplace_back(pR);
    ts_.emplace_back(pt);
    images_.emplace_back(cv::Mat(height, width, CV_8U, (void *)pimage).clone());
  }

  void SetDepthRange(double min_depth, double max_depth, int num_depth_planes) {
    min_depth_ = min_depth;
    max_depth_ = max_depth;
    num_depth_planes_ = num_depth_planes;
  }

  void ComputeBruteForce(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    *best_depth = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);
    *best_plane = cv::Mat(images_[0].rows, images_[0].cols, CV_32FC3, 0.0f);
    *best_score = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);

    int hpz = (patch_size_ - 1) / 2;
    for (int i = hpz; i < best_depth->rows - hpz; ++i) {
      std::cout << "i " << i << "\n";
      for (int j = hpz; j < best_depth->cols - hpz; ++j) {
        for (int d = 0; d < num_depth_planes_; ++d) {
          float depth = 1 / (1 / min_depth_ + d * (1 / max_depth_ - 1 / min_depth_) / (num_depth_planes_ - 1));
          cv::Vec3f normal(0, 0, -1);
          cv::Vec3f plane = PlaneFromDepthAndNormal(j, i, Ks_[0], depth, normal);
          CheckPlaneCandidate(best_depth, best_plane, best_score, i, j, plane);
        }
      }
    }
  }

  void ComputePatchMatch(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    *best_depth = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);
    *best_plane = cv::Mat(images_[0].rows, images_[0].cols, CV_32FC3, 0.0f);
    *best_score = cv::Mat(images_[0].rows, images_[0].cols, CV_32F, 0.0f);

    for (int i = 0; i < 3; ++i) {
      std::cout << "PatchMatchForwardPass " << i << "\n";
      PatchMatchForwardPass(best_depth, best_plane, best_score);
      std::cout << "PatchMatchBackwardPass " << i << "\n";
      PatchMatchBackwardPass(best_depth, best_plane, best_score);
    }
  }

  void PatchMatchForwardPass(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    int neighbors[4][2] = {{-1, -1}, {-1, 0}, {-1, 1}, {0, -1}};
    int hpz = (patch_size_ - 1) / 2;
    for (int i = hpz; i < best_depth->rows - hpz; ++i) {
      for (int j = hpz; j < best_depth->cols - hpz; ++j) {
        PatchMatchUpdatePixel(best_depth, best_plane, best_score, i, j, neighbors);
      }
    }
  }

  void PatchMatchBackwardPass(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score) {
    int neighbors[4][2] = {{0, 1}, {1, -1}, {1, 0}, {1, 1}};
    int hpz = (patch_size_ - 1) / 2;
    for (int i = best_depth->rows - hpz - 1; i >= hpz; --i) {
      for (int j = best_depth->cols - hpz - 1; j >= hpz; --j) {
        PatchMatchUpdatePixel(best_depth, best_plane, best_score, i, j, neighbors);
      }
    }
  }

  void PatchMatchUpdatePixel(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score,
                             int i, int j,
                             int neighbors[4][2]) {
    // Check neighbor's planes.
    for (int k = 0; k < 4; ++k) {
      cv::Vec3f plane = best_plane->at<cv::Vec3f>(i + neighbors[k][0], j + neighbors[k][1]);
      CheckPlaneCandidate(best_depth, best_plane, best_score, i, j, plane);
    }

    // Check random planes.
    for (int k = 0; k < 4; ++k) {
      float depth = 1 / (1 / min_depth_ + rand() * (1 / max_depth_ - 1 / min_depth_) / RAND_MAX);
      cv::Vec3f normal = RandomNormal();
      cv::Vec3f plane = PlaneFromDepthAndNormal(j, i, Ks_[0], depth, normal);
      CheckPlaneCandidate(best_depth, best_plane, best_score, i, j, plane);
    }
  }

  void CheckPlaneCandidate(cv::Mat *best_depth, cv::Mat *best_plane, cv::Mat *best_score,
                           int i, int j, const cv::Vec3f &plane) {
    float score = ComputePlaneScore(i, j, plane);
    if (score > best_score->at<float>(i, j)) {
      best_score->at<float>(i, j) = score;
      best_plane->at<cv::Vec3f>(i, j) = plane;
      best_depth->at<float>(i, j) = DepthOfPlaneBackprojection(j, i, Ks_[0], plane);
    }
  }

  float ComputePlaneScore(int i, int j, const cv::Vec3f &plane) {
    float best_score = -1.0f;
    for (int other = 1; other < images_.size(); ++other) {
      float score = ComputePlaneImageScore(i, j, plane, other);
      if (score > best_score) {
        best_score = score;
      }
    }
    return best_score;
  }

  float ComputePlaneImageScore(int i, int j,
                               const cv::Vec3f &plane,
                               int other) {
    cv::Matx33d H = PlaneInducedHomography(Ks_[0], Rs_[0], ts_[0],
                                           Ks_[other], Rs_[other], ts_[other],
                                           plane);
    int hpz = (patch_size_ - 1) / 2;
    float patch1[patch_size_ * patch_size_];
    float patch2[patch_size_ * patch_size_];
    int counter = 0;
    for (int u = -hpz; u <= hpz; ++u) {
      for (int v = -hpz; v <= hpz; ++v) {
        patch1[counter] = images_[0].at<unsigned char>(i + u, j + v);
        double x2, y2;
        ApplyHomography(H, j + v, i + u, &x2, &y2);
        patch2[counter] = LinearInterpolation(images_[other], y2, x2);
        counter++;
      }
    }
    return NormalizedCrossCorrelation(patch1, patch2, patch_size_ * patch_size_);
  }

 private:
  std::vector<cv::Mat> images_;
  std::vector<cv::Matx33d> Ks_;
  std::vector<cv::Matx33d> Rs_;
  std::vector<cv::Matx31d> ts_;
  int patch_size_;
  double min_depth_, max_depth_;
  int num_depth_planes_;
};

}

<|endoftext|>


<commit_before>/* -*- C++ -*- */
/*************************************************************************
 * Copyright(c) 1995~2005  Masaharu Goto (cint@pcroot.cern.ch)
 *
 * For the licensing terms see the file COPYING
 *
 ************************************************************************/

#include <stdio.h>
#include <string.h>

// badinit.c
char *system = "WinNT";
//______________________________________________________________________________
void ls(char *path=0)
{
#ifdef __CINT__
   char s[256] = (!strcmp(system, "WinNT")) ? "dir /w " : "ls ";
#else
   char s[256] = "dir /w ";
#endif
   if (path) strcat(s,path);
   // gSystem.Exec(s);
   printf("%s\n",s);
}

int main() {
  for(int i=0;i<2;i++) ls();
  return 0;
}

<commit_msg>Fix usage of string constant.<commit_after>/* -*- C++ -*- */
/*************************************************************************
 * Copyright(c) 1995~2005  Masaharu Goto (cint@pcroot.cern.ch)
 *
 * For the licensing terms see the file COPYING
 *
 ************************************************************************/

#include <cstdio>
#include <cstring>

using namespace std;

// badinit.c
const char* system = "WinNT";

//______________________________________________________________________________
void ls(const char* path = 0)
{
   // --
   char s[256];
   s[0] = '\0';
#ifdef __CINT__
   if (!strcmp(system, "WinNT")) {
      strcpy(s, "dir /w ");
   }
   else {
      strcpy(s, "ls ");
   }
#else // __CINT__
   strcpy(s, "dir /w ");
#endif // __CINT__
   if (path) {
      strcat(s, path);
   }
   //gSystem.Exec(s);
   printf("%s\n", s);
}

int main()
{
   for (int i = 0; i < 2; ++i) {
      ls();
   }
   return 0;
}

<|endoftext|>


<commit_before>#include "argumentlist.h"

#include "../testutil.h"

#include <cstring>
#include <iostream>

using namespace std;

static void test_stringValidation()
{
    {
        cstring emptyWithNull("");
        cstring emptyWithoutNull;

        TEST(!ArgumentList::isStringValid(emptyWithoutNull));
        TEST(ArgumentList::isStringValid(emptyWithNull));

        TEST(!ArgumentList::isObjectPathValid(emptyWithoutNull));
        TEST(!ArgumentList::isObjectPathValid(emptyWithNull));

        TEST(ArgumentList::isSignatureValid(emptyWithNull));
        TEST(!ArgumentList::isSignatureValid(emptyWithoutNull));
        TEST(ArgumentList::isSignatureValid(emptyWithNull, ArgumentList::VariantSignature));
        TEST(!ArgumentList::isSignatureValid(emptyWithoutNull, ArgumentList::VariantSignature));
    }
    {
        cstring trivial("i");
        TEST(ArgumentList::isSignatureValid(trivial));
        TEST(ArgumentList::isSignatureValid(trivial, ArgumentList::VariantSignature));
    }
    {
        cstring list("iqb");
        TEST(ArgumentList::isSignatureValid(list));
        TEST(!ArgumentList::isSignatureValid(list, ArgumentList::VariantSignature));
        cstring list2("aii");
        TEST(ArgumentList::isSignatureValid(list2));
        TEST(!ArgumentList::isSignatureValid(list2, ArgumentList::VariantSignature));
    }
    {
        cstring simpleArray("ai");
        TEST(ArgumentList::isSignatureValid(simpleArray));
        TEST(ArgumentList::isSignatureValid(simpleArray, ArgumentList::VariantSignature));
    }
    {
        cstring messyArray("a(iaia{ia{iv}})");
        TEST(ArgumentList::isSignatureValid(messyArray));
        TEST(ArgumentList::isSignatureValid(messyArray, ArgumentList::VariantSignature));
    }
    {
        cstring dictFail("a{vi}");
        TEST(!ArgumentList::isSignatureValid(dictFail));
        TEST(!ArgumentList::isSignatureValid(dictFail, ArgumentList::VariantSignature));
    }
    {
        cstring emptyStruct("()");
        TEST(!ArgumentList::isSignatureValid(emptyStruct));
        TEST(!ArgumentList::isSignatureValid(emptyStruct, ArgumentList::VariantSignature));
        cstring emptyStruct2("(())");
        TEST(!ArgumentList::isSignatureValid(emptyStruct2));
        TEST(!ArgumentList::isSignatureValid(emptyStruct2, ArgumentList::VariantSignature));
        cstring miniStruct("(t)");
        TEST(ArgumentList::isSignatureValid(miniStruct));
        TEST(ArgumentList::isSignatureValid(miniStruct, ArgumentList::VariantSignature));
        cstring badStruct("(()");
        TEST(!ArgumentList::isSignatureValid(badStruct));
        TEST(!ArgumentList::isSignatureValid(badStruct, ArgumentList::VariantSignature));
        cstring badStruct2("())");
        TEST(!ArgumentList::isSignatureValid(badStruct2));
        TEST(!ArgumentList::isSignatureValid(badStruct2, ArgumentList::VariantSignature));
    }
    {
        cstring nullStr;
        cstring emptyStr("");
        TEST(!ArgumentList::isObjectPathValid(nullStr));
        TEST(!ArgumentList::isObjectPathValid(emptyStr));
        TEST(ArgumentList::isObjectPathValid(cstring("/")));
        TEST(!ArgumentList::isObjectPathValid(cstring("/abc/")));
        TEST(ArgumentList::isObjectPathValid(cstring("/abc")));
        TEST(ArgumentList::isObjectPathValid(cstring("/abc/def")));
        TEST(!ArgumentList::isObjectPathValid(cstring("/abc&def")));
        TEST(!ArgumentList::isObjectPathValid(cstring("/abc//def")));
        TEST(ArgumentList::isObjectPathValid(cstring("/aZ/0123_zAZa9_/_")));
    }
    {
        cstring maxStruct("((((((((((((((((((((((((((((((((i"
                          "))))))))))))))))))))))))))))))))");
        TEST(ArgumentList::isSignatureValid(maxStruct));
        TEST(ArgumentList::isSignatureValid(maxStruct, ArgumentList::VariantSignature));
        cstring struct33("(((((((((((((((((((((((((((((((((i" // too much nesting by one
                         ")))))))))))))))))))))))))))))))))");
        TEST(!ArgumentList::isSignatureValid(struct33));
        TEST(!ArgumentList::isSignatureValid(struct33, ArgumentList::VariantSignature));

        cstring maxArray("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaai");
        TEST(ArgumentList::isSignatureValid(maxArray));
        TEST(ArgumentList::isSignatureValid(maxArray, ArgumentList::VariantSignature));
        cstring array33("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaai");
        TEST(!ArgumentList::isSignatureValid(array33));
        TEST(!ArgumentList::isSignatureValid(array33, ArgumentList::VariantSignature));
    }
}

static bool arraysEqual(array a1, array a2)
{
    if (a1.length != a2.length) {
        return false;
    }
    for (int i = 0; i < a1.length; i++) {
        if (a1.begin[i] != a2.begin[i]) {
            return false;
        }
    }
    return true;
}

static bool stringsEqual(cstring s1, cstring s2)
{
    return arraysEqual(array(s1.begin, s1.length), array(s2.begin, s2.length));
}

static void doRoundtrip(ArgumentList arg)
{
    ArgumentList::ReadCursor reader = arg.beginRead();
    {
        ArgumentList::ReadCursor reader2 = arg.beginRead();
        TEST(reader2.isValid());
    }

    ArgumentList copy;
    ArgumentList::WriteCursor writer = copy.beginWrite();
    {
        ArgumentList::WriteCursor writer2 = copy.beginWrite();
        TEST(!writer2.isValid());
    }
    {
        ArgumentList::ReadCursor reader3 = copy.beginRead();
        TEST(!reader3.isValid());
    }

    bool isDone = false;
    while (!isDone) {
        TEST(writer.state() != ArgumentList::InvalidData);
        cerr << "Reader state: " << reader.state() << endl;

        switch(reader.state()) {
        case ArgumentList::Finished:
            writer.finish();
            isDone = true;
            break;
        case ArgumentList::NeedMoreData:
            TEST(false);
            break;
        case ArgumentList::BeginStruct:
            reader.beginStruct();
            writer.beginStruct();
            break;
        case ArgumentList::EndStruct:
            reader.endStruct();
            writer.endStruct();
            break;
        case ArgumentList::BeginVariant:
            reader.beginVariant();
            writer.beginVariant();
            break;
        case ArgumentList::EndVariant:
            reader.endVariant();
            writer.endVariant();
            break;
        case ArgumentList::BeginArray: {
            bool isEmpty;
            reader.beginArray(&isEmpty);
            writer.beginArray(isEmpty);
            break; }
        case ArgumentList::NextArrayEntry:
            if (reader.nextArrayEntry()) {
                writer.nextArrayEntry();
            } else {
                writer.endArray();
            }
            break;
        case ArgumentList::EndArray:
            reader.endArray();
            // writer.endArray(); // already done when reader.nextArrayEntry() returns false
            break;
        case ArgumentList::BeginDict: {
            bool isEmpty;
            reader.beginDict(&isEmpty);
            writer.beginDict(isEmpty);
            break; }
        case ArgumentList::NextDictEntry:
            if (reader.nextDictEntry()) {
                writer.nextDictEntry();
            } else {
                writer.endDict();
            }
            break;
        case ArgumentList::EndDict:
            reader.endDict();
            // writer.endDict(); // already done when reader.nextDictEntry() returns false
            break;
        case ArgumentList::Byte:
            writer.writeByte(reader.readByte());
            break;
        case ArgumentList::Boolean:
            writer.writeBoolean(reader.readBoolean());
            break;
        case ArgumentList::Int16:
            writer.writeInt16(reader.readInt16());
            break;
        case ArgumentList::Uint16:
            writer.writeUint16(reader.readUint16());
            break;
        case ArgumentList::Int32:
            writer.writeInt32(reader.readInt32());
            break;
        case ArgumentList::Uint32:
            writer.writeUint32(reader.readUint32());
            break;
        case ArgumentList::Int64:
            writer.writeInt64(reader.readInt64());
            break;
        case ArgumentList::Uint64:
            writer.writeUint64(reader.readUint64());
            break;
        case ArgumentList::Double:
            writer.writeDouble(reader.readDouble());
            break;
        case ArgumentList::String:
            writer.writeString(reader.readString());
            break;
        case ArgumentList::ObjectPath:
            writer.writeObjectPath(reader.readObjectPath());
            break;
        case ArgumentList::Signature:
            writer.writeSignature(reader.readSignature());
            break;
        case ArgumentList::UnixFd:
            writer.writeUnixFd(reader.readUnixFd());
            break;
        default:
            TEST(false);
            break;
        }
    }
    cstring argSignature = arg.signature();
    cstring copySignature = copy.signature();
    TEST(ArgumentList::isSignatureValid(copySignature));
    TEST(stringsEqual(argSignature, copySignature));

    array argData = arg.data();
    array copyData = copy.data();
    TEST(arraysEqual(argData, copyData));
}

void test_roundtrip()
{
    doRoundtrip(ArgumentList(cstring(""), array()));
}

int main(int argc, char *argv[])
{
    test_stringValidation();
    test_roundtrip();
}
<commit_msg>The first non-empty roundtrip test, currently fails.<commit_after>#include "argumentlist.h"

#include "../testutil.h"

#include <cstring>
#include <iostream>

using namespace std;

static void test_stringValidation()
{
    {
        cstring emptyWithNull("");
        cstring emptyWithoutNull;

        TEST(!ArgumentList::isStringValid(emptyWithoutNull));
        TEST(ArgumentList::isStringValid(emptyWithNull));

        TEST(!ArgumentList::isObjectPathValid(emptyWithoutNull));
        TEST(!ArgumentList::isObjectPathValid(emptyWithNull));

        TEST(ArgumentList::isSignatureValid(emptyWithNull));
        TEST(!ArgumentList::isSignatureValid(emptyWithoutNull));
        TEST(ArgumentList::isSignatureValid(emptyWithNull, ArgumentList::VariantSignature));
        TEST(!ArgumentList::isSignatureValid(emptyWithoutNull, ArgumentList::VariantSignature));
    }
    {
        cstring trivial("i");
        TEST(ArgumentList::isSignatureValid(trivial));
        TEST(ArgumentList::isSignatureValid(trivial, ArgumentList::VariantSignature));
    }
    {
        cstring list("iqb");
        TEST(ArgumentList::isSignatureValid(list));
        TEST(!ArgumentList::isSignatureValid(list, ArgumentList::VariantSignature));
        cstring list2("aii");
        TEST(ArgumentList::isSignatureValid(list2));
        TEST(!ArgumentList::isSignatureValid(list2, ArgumentList::VariantSignature));
    }
    {
        cstring simpleArray("ai");
        TEST(ArgumentList::isSignatureValid(simpleArray));
        TEST(ArgumentList::isSignatureValid(simpleArray, ArgumentList::VariantSignature));
    }
    {
        cstring messyArray("a(iaia{ia{iv}})");
        TEST(ArgumentList::isSignatureValid(messyArray));
        TEST(ArgumentList::isSignatureValid(messyArray, ArgumentList::VariantSignature));
    }
    {
        cstring dictFail("a{vi}");
        TEST(!ArgumentList::isSignatureValid(dictFail));
        TEST(!ArgumentList::isSignatureValid(dictFail, ArgumentList::VariantSignature));
    }
    {
        cstring emptyStruct("()");
        TEST(!ArgumentList::isSignatureValid(emptyStruct));
        TEST(!ArgumentList::isSignatureValid(emptyStruct, ArgumentList::VariantSignature));
        cstring emptyStruct2("(())");
        TEST(!ArgumentList::isSignatureValid(emptyStruct2));
        TEST(!ArgumentList::isSignatureValid(emptyStruct2, ArgumentList::VariantSignature));
        cstring miniStruct("(t)");
        TEST(ArgumentList::isSignatureValid(miniStruct));
        TEST(ArgumentList::isSignatureValid(miniStruct, ArgumentList::VariantSignature));
        cstring badStruct("(()");
        TEST(!ArgumentList::isSignatureValid(badStruct));
        TEST(!ArgumentList::isSignatureValid(badStruct, ArgumentList::VariantSignature));
        cstring badStruct2("())");
        TEST(!ArgumentList::isSignatureValid(badStruct2));
        TEST(!ArgumentList::isSignatureValid(badStruct2, ArgumentList::VariantSignature));
    }
    {
        cstring nullStr;
        cstring emptyStr("");
        TEST(!ArgumentList::isObjectPathValid(nullStr));
        TEST(!ArgumentList::isObjectPathValid(emptyStr));
        TEST(ArgumentList::isObjectPathValid(cstring("/")));
        TEST(!ArgumentList::isObjectPathValid(cstring("/abc/")));
        TEST(ArgumentList::isObjectPathValid(cstring("/abc")));
        TEST(ArgumentList::isObjectPathValid(cstring("/abc/def")));
        TEST(!ArgumentList::isObjectPathValid(cstring("/abc&def")));
        TEST(!ArgumentList::isObjectPathValid(cstring("/abc//def")));
        TEST(ArgumentList::isObjectPathValid(cstring("/aZ/0123_zAZa9_/_")));
    }
    {
        cstring maxStruct("((((((((((((((((((((((((((((((((i"
                          "))))))))))))))))))))))))))))))))");
        TEST(ArgumentList::isSignatureValid(maxStruct));
        TEST(ArgumentList::isSignatureValid(maxStruct, ArgumentList::VariantSignature));
        cstring struct33("(((((((((((((((((((((((((((((((((i" // too much nesting by one
                         ")))))))))))))))))))))))))))))))))");
        TEST(!ArgumentList::isSignatureValid(struct33));
        TEST(!ArgumentList::isSignatureValid(struct33, ArgumentList::VariantSignature));

        cstring maxArray("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaai");
        TEST(ArgumentList::isSignatureValid(maxArray));
        TEST(ArgumentList::isSignatureValid(maxArray, ArgumentList::VariantSignature));
        cstring array33("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaai");
        TEST(!ArgumentList::isSignatureValid(array33));
        TEST(!ArgumentList::isSignatureValid(array33, ArgumentList::VariantSignature));
    }
}

static bool arraysEqual(array a1, array a2)
{
    if (a1.length != a2.length) {
        return false;
    }
    for (int i = 0; i < a1.length; i++) {
        if (a1.begin[i] != a2.begin[i]) {
            return false;
        }
    }
    return true;
}

static bool stringsEqual(cstring s1, cstring s2)
{
    return arraysEqual(array(s1.begin, s1.length), array(s2.begin, s2.length));
}

static void doRoundtrip(ArgumentList arg)
{
    ArgumentList::ReadCursor reader = arg.beginRead();
    {
        ArgumentList::ReadCursor reader2 = arg.beginRead();
        TEST(reader2.isValid());
    }

    ArgumentList copy;
    ArgumentList::WriteCursor writer = copy.beginWrite();
    {
        ArgumentList::WriteCursor writer2 = copy.beginWrite();
        TEST(!writer2.isValid());
    }
    {
        ArgumentList::ReadCursor reader3 = copy.beginRead();
        TEST(!reader3.isValid());
    }

    bool isDone = false;
    while (!isDone) {
        TEST(writer.state() != ArgumentList::InvalidData);
        cerr << "Reader state: " << reader.state() << endl;

        switch(reader.state()) {
        case ArgumentList::Finished:
            writer.finish();
            isDone = true;
            break;
        case ArgumentList::NeedMoreData:
            TEST(false);
            break;
        case ArgumentList::BeginStruct:
            reader.beginStruct();
            writer.beginStruct();
            break;
        case ArgumentList::EndStruct:
            reader.endStruct();
            writer.endStruct();
            break;
        case ArgumentList::BeginVariant:
            reader.beginVariant();
            writer.beginVariant();
            break;
        case ArgumentList::EndVariant:
            reader.endVariant();
            writer.endVariant();
            break;
        case ArgumentList::BeginArray: {
            bool isEmpty;
            reader.beginArray(&isEmpty);
            writer.beginArray(isEmpty);
            break; }
        case ArgumentList::NextArrayEntry:
            if (reader.nextArrayEntry()) {
                writer.nextArrayEntry();
            } else {
                writer.endArray();
            }
            break;
        case ArgumentList::EndArray:
            reader.endArray();
            // writer.endArray(); // already done when reader.nextArrayEntry() returns false
            break;
        case ArgumentList::BeginDict: {
            bool isEmpty;
            reader.beginDict(&isEmpty);
            writer.beginDict(isEmpty);
            break; }
        case ArgumentList::NextDictEntry:
            if (reader.nextDictEntry()) {
                writer.nextDictEntry();
            } else {
                writer.endDict();
            }
            break;
        case ArgumentList::EndDict:
            reader.endDict();
            // writer.endDict(); // already done when reader.nextDictEntry() returns false
            break;
        case ArgumentList::Byte:
            writer.writeByte(reader.readByte());
            break;
        case ArgumentList::Boolean:
            writer.writeBoolean(reader.readBoolean());
            break;
        case ArgumentList::Int16:
            writer.writeInt16(reader.readInt16());
            break;
        case ArgumentList::Uint16:
            writer.writeUint16(reader.readUint16());
            break;
        case ArgumentList::Int32:
            writer.writeInt32(reader.readInt32());
            break;
        case ArgumentList::Uint32:
            writer.writeUint32(reader.readUint32());
            break;
        case ArgumentList::Int64:
            writer.writeInt64(reader.readInt64());
            break;
        case ArgumentList::Uint64:
            writer.writeUint64(reader.readUint64());
            break;
        case ArgumentList::Double:
            writer.writeDouble(reader.readDouble());
            break;
        case ArgumentList::String:
            writer.writeString(reader.readString());
            break;
        case ArgumentList::ObjectPath:
            writer.writeObjectPath(reader.readObjectPath());
            break;
        case ArgumentList::Signature:
            writer.writeSignature(reader.readSignature());
            break;
        case ArgumentList::UnixFd:
            writer.writeUnixFd(reader.readUnixFd());
            break;
        default:
            TEST(false);
            break;
        }
    }
    cstring argSignature = arg.signature();
    cstring copySignature = copy.signature();
    TEST(ArgumentList::isSignatureValid(copySignature));
    TEST(stringsEqual(argSignature, copySignature));

    array argData = arg.data();
    array copyData = copy.data();
    TEST(arraysEqual(argData, copyData));
}

void test_roundtrip()
{
    doRoundtrip(ArgumentList(cstring(""), array()));
    {
        byte data[4] = { 1, 2, 3, 4 }; // no idea which integer that works out to :)
        doRoundtrip(ArgumentList(cstring("i"), array(data, 4)));
    }
}

int main(int argc, char *argv[])
{
    test_stringValidation();
    test_roundtrip();
}
<|endoftext|>


<commit_before>#include "PathTask.hpp"
#include "Robosub/HighLevelControl.h"
#include <stdio.h>
#include <math.h>
#include <float.h>

/**
 * @brief The constructor. Subscribes to all needed topics and sets up callbacks
 */
PathTask::PathTask()
 : m_nodeHandle(),
   m_pathSubscriber(),
   m_taskStateSubscriber(),
   m_pathDirSubscriber(),
   m_highLevelMotorPublisher(),
   m_taskCompletePublisher(),
   m_isEnabled(false),
   m_frames(0),
   m_direction("left"),
   m_msgList(),
   m_lastId(-1)
{
   printf("setup\n");
   fflush(NULL);
   m_pathSubscriber = m_nodeHandle.subscribe("img_rec/paths", 10, &PathTask::pathCallback, this);
   m_taskStateSubscriber = m_nodeHandle.subscribe("Module_Enable", 10, &PathTask::moduleEnableCallback, this);
   m_pathDirSubscriber = m_nodeHandle.subscribe("Path_Direction", 10, &PathTask::pathDirectionCallback, this);
   m_highLevelMotorPublisher = m_nodeHandle.advertise<Robosub::HighLevelControl>("High_Level_Motion", 10);
   m_taskCompletePublisher = m_nodeHandle.advertise<std_msgs::String>("Task_Completion", 10);
}

/**
 * @brief The destructor
 */
PathTask::~PathTask()
{

}

/**
 * @brief The module enable message callback
 *
 * @param msg The module enabled message
 */
void PathTask::moduleEnableCallback(const Robosub::ModuleEnableMsg& msg)
{
  if (msg.Module == "PathTask")
  {
    if (msg.State == true)
    {
      printf("PathTask: enabled\n");
      m_isEnabled = true;
      m_frames = 0;
      m_msgList.clear();
    }
    else
    {
      printf("PathTask: Disabled\n");
      m_isEnabled = false;
    }
  }
}

void PathTask::pathDirectionCallback(const std_msgs::String& msg)
{
  m_direction = msg.data;
}

/**
 * @brief The path callback
 *
 * @param msg The image recognition object message
 */
void PathTask::pathCallback(const SubImageRecognition::ImgRecObject& msg)
{
	if (m_isEnabled)
	{
	  if (msg.id == 0 && m_lastId != -1)
	  {
	    //find the correct target
	    SubImageRecognition::ImgRecObject tmp = getMessage();

	    manualMode(msg);
	    //offsetMode(msg);
	    m_msgList.clear();
	    m_msgList.push_back(msg);
	  }
	  else
	  {
	    m_msgList.push_back(msg);
	  }

    m_lastId = msg.id;
	}
}

void PathTask::manualMode(const SubImageRecognition::ImgRecObject& msg)
{
  int hit = 0;
  float moveX = msg.center_x;
  float moveY = msg.center_y;
  float turn = msg.rotation;

  if (moveX > 10 || moveX < -10)
  {
    printf("Correcting straf by: %f\n", moveX);
    publishMotor("Straf", "Manual", moveX*2.0);
    hit++;
  }
  else
  {
    publishMotor("Straf", "Manual", 0.0);
  }

  if (moveY > 10 || moveY < -10)
  {
    printf("Correcting forward by: %f\n", moveY);
    publishMotor("Forward", "Manual", moveY*2.0);
    hit++;
  }
  else
  {
    publishMotor("Forward", "Manual", 0.0);
  }

  if (turn > 1 || turn < -1)
  {
    printf("Correcting turn by: %f\n", turn);
    publishMotor("Turn", "Manual", turn);
    hit++;
  }
  else
  {
    publishMotor("Turn", "Manual", 0.0);
  }

  if (hit == 0)
  {
    m_frames++;
  }
  else
  {
    m_frames = 0;
  }

  if (m_frames >= 10)
  {
    reportSuccess(true);
    m_frames = 0;
    m_isEnabled = false;
  }
}

void PathTask::offsetMode(const SubImageRecognition::ImgRecObject& msg)
{
  int hit = 0;
  float moveX = calculateDistanceFromCenter(msg.center_x, msg.width);
  float moveY = calculateDistanceFromCenter(msg.center_y, msg.width);
  float turn = msg.rotation;

  if (moveX > 10 || moveX < -10)
  {
    printf("Correcting straf by: %f\n", moveX);
    publishMotor("Straf", "Offset", moveX > 0 ? 1 : -1); //inches to feet
    hit++;
  }
  else
  {
    publishMotor("Straf", "Offset", 0.0);
  }

  if (moveY > 10 || moveY < -10)
  {
    printf("Correcting forward by: %f\n", moveY);
    publishMotor("Forward", "Offset", moveY > 0 ? 1 : -1);
    hit++;
  }
  else
  {
    publishMotor("Forward", "Offset", 0.0);
  }

  if (turn > 1 || turn < -1)
  {
    printf("Correcting turn by: %f\n", turn);
    publishMotor("Turn", "Offset", turn);
    hit++;
  }
  else
  {
    publishMotor("Turn", "Offset", 0.0);
  }

  if (hit == 0)
  {
    m_frames++;
  }
  else
  {
    m_frames = 0;
  }

  if (m_frames >= 10)
  {
    reportSuccess(true);
    m_frames = 0;
    m_isEnabled = false;
  }
}

SubImageRecognition::ImgRecObject PathTask::getMessage()
{
  SubImageRecognition::ImgRecObject ret;

  if (m_direction == "right")
  {
    int greatest = SHRT_MIN;
    for (std::list<SubImageRecognition::ImgRecObject>::iterator it = m_msgList.begin(); it != m_msgList.end(); it++)
    {
      if (it->center_x > greatest)
      {
        greatest = it->center_x;
        ret = *it;
      }
    }
  }
  else if (m_direction == "left")
  {
    int greatest = SHRT_MAX;
    for (std::list<SubImageRecognition::ImgRecObject>::iterator it = m_msgList.begin(); it != m_msgList.end(); it++)
    {
      if (it->center_x < greatest)
      {
        greatest = it->center_x;
        ret = *it;
      }
    }
  }
  else
  {
    if (m_msgList.size() > 0)
    {
      ret = m_msgList.front();
    }
  }

  return ret;
}

/**
 * @brief report the success or failure of the module
 *
 * @param success The success or failure
 */
void PathTask::reportSuccess(bool success)
{
  std_msgs::String msg;
  msg.data = "PathTask";
  if (success)
  {
    msg.data += " Success";
  }
  else
  {
    msg.data = " Failure";
  }
  m_taskCompletePublisher.publish(msg);
}

/**
 * @brief plubish to the high level motor controller topic
 *
 * @param direction The direction: Forward, Straf, Turn
 * @param motion The motion type: Manual, Offset
 * @param value The value
 */
void PathTask::publishMotor(std::string direction, std::string motion, float value)
{
  Robosub::HighLevelControl msg;
  msg.Direction = direction;
  msg.MotionType = motion;
  msg.Value = value;

  m_highLevelMotorPublisher.publish(msg);
}

/**
 * @brief Calcualte the distance the center of the object is off from the center of the camera in inches
 *
 * @param centerDir The current center direction
 * @param width The width of the object
 *
 * @return The distance in inches
 */
float PathTask::calculateDistanceFromCenter(float centerDir, float width)
{
  float pxlPerInch = getPixelsPerInch(width, 6.0f);
  return centerDir * pxlPerInch;
}

/**
 * @brief Get the number of pixels per inch
 *
 * @param curWidthPixels The current width of a known object in pixels
 * @param expectedWidthInches The expected with of an object in inches
 *
 * @return The number of pixels per inch if everything goes well, or float_max if inputs were unacceptable
 */
float PathTask::getPixelsPerInch(float curWidthPixels, float expectedWidthInches)
{
  float dist = getDistance(curWidthPixels, expectedWidthInches);
  return curWidthPixels/dist;
}

/**
 * @brief Calculate the distance to the object
 *
 * @param curObjSize The objects current size
 * @param actualObjSize The objects expected size
 *
 * @return The distance
 */
float PathTask::getDistance(float curObjSize, float actualObjSize)
{
  return (actualObjSize/(2.0f*tan(curObjSize * (M_PI/4.0f)/960.0f))); //45 degrees
}
<commit_msg>removed multiplier<commit_after>#include "PathTask.hpp"
#include "Robosub/HighLevelControl.h"
#include <stdio.h>
#include <math.h>
#include <float.h>

/**
 * @brief The constructor. Subscribes to all needed topics and sets up callbacks
 */
PathTask::PathTask()
 : m_nodeHandle(),
   m_pathSubscriber(),
   m_taskStateSubscriber(),
   m_pathDirSubscriber(),
   m_highLevelMotorPublisher(),
   m_taskCompletePublisher(),
   m_isEnabled(false),
   m_frames(0),
   m_direction("left"),
   m_msgList(),
   m_lastId(-1)
{
   printf("setup\n");
   fflush(NULL);
   m_pathSubscriber = m_nodeHandle.subscribe("img_rec/paths", 10, &PathTask::pathCallback, this);
   m_taskStateSubscriber = m_nodeHandle.subscribe("Module_Enable", 10, &PathTask::moduleEnableCallback, this);
   m_pathDirSubscriber = m_nodeHandle.subscribe("Path_Direction", 10, &PathTask::pathDirectionCallback, this);
   m_highLevelMotorPublisher = m_nodeHandle.advertise<Robosub::HighLevelControl>("High_Level_Motion", 10);
   m_taskCompletePublisher = m_nodeHandle.advertise<std_msgs::String>("Task_Completion", 10);
}

/**
 * @brief The destructor
 */
PathTask::~PathTask()
{

}

/**
 * @brief The module enable message callback
 *
 * @param msg The module enabled message
 */
void PathTask::moduleEnableCallback(const Robosub::ModuleEnableMsg& msg)
{
  if (msg.Module == "PathTask")
  {
    if (msg.State == true)
    {
      printf("PathTask: enabled\n");
      m_isEnabled = true;
      m_frames = 0;
      m_msgList.clear();
    }
    else
    {
      printf("PathTask: Disabled\n");
      m_isEnabled = false;
    }
  }
}

void PathTask::pathDirectionCallback(const std_msgs::String& msg)
{
  m_direction = msg.data;
}

/**
 * @brief The path callback
 *
 * @param msg The image recognition object message
 */
void PathTask::pathCallback(const SubImageRecognition::ImgRecObject& msg)
{
	if (m_isEnabled)
	{
	  if (msg.id == 0 && m_lastId != -1)
	  {
	    //find the correct target
	    SubImageRecognition::ImgRecObject tmp = getMessage();

	    manualMode(msg);
	    //offsetMode(msg);
	    m_msgList.clear();
	    m_msgList.push_back(msg);
	  }
	  else
	  {
	    m_msgList.push_back(msg);
	  }

    m_lastId = msg.id;
	}
}

void PathTask::manualMode(const SubImageRecognition::ImgRecObject& msg)
{
  int hit = 0;
  float moveX = msg.center_x;
  float moveY = msg.center_y;
  float turn = msg.rotation;

  if (moveX > 10 || moveX < -10)
  {
    printf("Correcting straf by: %f\n", moveX);
    publishMotor("Straf", "Manual", moveX);
    hit++;
  }
  else
  {
    publishMotor("Straf", "Manual", 0.0);
  }

  if (moveY > 10 || moveY < -10)
  {
    printf("Correcting forward by: %f\n", moveY);
    publishMotor("Forward", "Manual", moveY*2.0);
    hit++;
  }
  else
  {
    publishMotor("Forward", "Manual", 0.0);
  }

  if (turn > 1 || turn < -1)
  {
    printf("Correcting turn by: %f\n", turn);
    publishMotor("Turn", "Manual", turn);
    hit++;
  }
  else
  {
    publishMotor("Turn", "Manual", 0.0);
  }

  if (hit == 0)
  {
    m_frames++;
  }
  else
  {
    m_frames = 0;
  }

  if (m_frames >= 10)
  {
    reportSuccess(true);
    m_frames = 0;
    m_isEnabled = false;
  }
}

void PathTask::offsetMode(const SubImageRecognition::ImgRecObject& msg)
{
  int hit = 0;
  float moveX = calculateDistanceFromCenter(msg.center_x, msg.width);
  float moveY = calculateDistanceFromCenter(msg.center_y, msg.width);
  float turn = msg.rotation;

  if (moveX > 10 || moveX < -10)
  {
    printf("Correcting straf by: %f\n", moveX);
    publishMotor("Straf", "Offset", moveX > 0 ? 1 : -1); //inches to feet
    hit++;
  }
  else
  {
    publishMotor("Straf", "Offset", 0.0);
  }

  if (moveY > 10 || moveY < -10)
  {
    printf("Correcting forward by: %f\n", moveY);
    publishMotor("Forward", "Offset", moveY > 0 ? 1 : -1);
    hit++;
  }
  else
  {
    publishMotor("Forward", "Offset", 0.0);
  }

  if (turn > 1 || turn < -1)
  {
    printf("Correcting turn by: %f\n", turn);
    publishMotor("Turn", "Offset", turn);
    hit++;
  }
  else
  {
    publishMotor("Turn", "Offset", 0.0);
  }

  if (hit == 0)
  {
    m_frames++;
  }
  else
  {
    m_frames = 0;
  }

  if (m_frames >= 10)
  {
    reportSuccess(true);
    m_frames = 0;
    m_isEnabled = false;
  }
}

SubImageRecognition::ImgRecObject PathTask::getMessage()
{
  SubImageRecognition::ImgRecObject ret;

  if (m_direction == "right")
  {
    int greatest = SHRT_MIN;
    for (std::list<SubImageRecognition::ImgRecObject>::iterator it = m_msgList.begin(); it != m_msgList.end(); it++)
    {
      if (it->center_x > greatest)
      {
        greatest = it->center_x;
        ret = *it;
      }
    }
  }
  else if (m_direction == "left")
  {
    int greatest = SHRT_MAX;
    for (std::list<SubImageRecognition::ImgRecObject>::iterator it = m_msgList.begin(); it != m_msgList.end(); it++)
    {
      if (it->center_x < greatest)
      {
        greatest = it->center_x;
        ret = *it;
      }
    }
  }
  else
  {
    if (m_msgList.size() > 0)
    {
      ret = m_msgList.front();
    }
  }

  return ret;
}

/**
 * @brief report the success or failure of the module
 *
 * @param success The success or failure
 */
void PathTask::reportSuccess(bool success)
{
  std_msgs::String msg;
  msg.data = "PathTask";
  if (success)
  {
    msg.data += " Success";
  }
  else
  {
    msg.data = " Failure";
  }
  m_taskCompletePublisher.publish(msg);
}

/**
 * @brief plubish to the high level motor controller topic
 *
 * @param direction The direction: Forward, Straf, Turn
 * @param motion The motion type: Manual, Offset
 * @param value The value
 */
void PathTask::publishMotor(std::string direction, std::string motion, float value)
{
  Robosub::HighLevelControl msg;
  msg.Direction = direction;
  msg.MotionType = motion;
  msg.Value = value;

  m_highLevelMotorPublisher.publish(msg);
}

/**
 * @brief Calcualte the distance the center of the object is off from the center of the camera in inches
 *
 * @param centerDir The current center direction
 * @param width The width of the object
 *
 * @return The distance in inches
 */
float PathTask::calculateDistanceFromCenter(float centerDir, float width)
{
  float pxlPerInch = getPixelsPerInch(width, 6.0f);
  return centerDir * pxlPerInch;
}

/**
 * @brief Get the number of pixels per inch
 *
 * @param curWidthPixels The current width of a known object in pixels
 * @param expectedWidthInches The expected with of an object in inches
 *
 * @return The number of pixels per inch if everything goes well, or float_max if inputs were unacceptable
 */
float PathTask::getPixelsPerInch(float curWidthPixels, float expectedWidthInches)
{
  float dist = getDistance(curWidthPixels, expectedWidthInches);
  return curWidthPixels/dist;
}

/**
 * @brief Calculate the distance to the object
 *
 * @param curObjSize The objects current size
 * @param actualObjSize The objects expected size
 *
 * @return The distance
 */
float PathTask::getDistance(float curObjSize, float actualObjSize)
{
  return (actualObjSize/(2.0f*tan(curObjSize * (M_PI/4.0f)/960.0f))); //45 degrees
}
<|endoftext|>


<commit_before>//==- GRCoreEngine.cpp - Path-Sensitive Dataflow Engine ----------------*- C++ -*-//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
//  This file defines a generic engine for intraprocedural, path-sensitive,
//  dataflow analysis via graph reachability engine.
//
//===----------------------------------------------------------------------===//

#include "clang/Analysis/PathSensitive/GRCoreEngine.h"
#include "clang/AST/Expr.h"
#include "llvm/Support/Compiler.h"
#include "llvm/Support/Casting.h"
#include "llvm/ADT/DenseMap.h"
#include <vector>

using llvm::cast;
using llvm::isa;
using namespace clang;

namespace {
  class VISIBILITY_HIDDEN DFS : public GRWorkList {
  llvm::SmallVector<GRWorkListUnit,20> Stack;
public:
  virtual bool hasWork() const {
    return !Stack.empty();
  }

  virtual void Enqueue(const GRWorkListUnit& U) {
    Stack.push_back(U);
  }

  virtual GRWorkListUnit Dequeue() {
    assert (!Stack.empty());
    const GRWorkListUnit& U = Stack.back();
    Stack.pop_back(); // This technically "invalidates" U, but we are fine.
    return U;
  }
};
} // end anonymous namespace

// Place the dstor for GRWorkList here because it contains virtual member
// functions, and we the code for the dstor generated in one compilation unit.
GRWorkList::~GRWorkList() {}

GRWorkList* GRWorkList::MakeDFS() { return new DFS(); }

/// ExecuteWorkList - Run the worklist algorithm for a maximum number of steps.
bool GRCoreEngineImpl::ExecuteWorkList(unsigned Steps) {

  if (G->num_roots() == 0) { // Initialize the analysis by constructing
    // the root if none exists.

    CFGBlock* Entry = &getCFG().getEntry();

    assert (Entry->empty() &&
            "Entry block must be empty.");

    assert (Entry->succ_size() == 1 &&
            "Entry block must have 1 successor.");

    // Get the solitary successor.
    CFGBlock* Succ = *(Entry->succ_begin());

    // Construct an edge representing the
    // starting location in the function.
    BlockEdge StartLoc(getCFG(), Entry, Succ);

    // Set the current block counter to being empty.
    WList->setBlockCounter(BCounterFactory.GetEmptyCounter());

    // Generate the root.
    GenerateNode(StartLoc, getInitialState());
  }

  while (Steps && WList->hasWork()) {
    --Steps;
    const GRWorkListUnit& WU = WList->Dequeue();

    // Set the current block counter.
    WList->setBlockCounter(WU.getBlockCounter());

    // Retrieve the node.
    ExplodedNodeImpl* Node = WU.getNode();

    // Dispatch on the location type.
    switch (Node->getLocation().getKind()) {
      default:
        assert (isa<BlockEdge>(Node->getLocation()));
        HandleBlockEdge(cast<BlockEdge>(Node->getLocation()), Node);
        break;

      case ProgramPoint::BlockEntranceKind:
        HandleBlockEntrance(cast<BlockEntrance>(Node->getLocation()), Node);
        break;

      case ProgramPoint::BlockExitKind:
        assert (false && "BlockExit location never occur in forward analysis.");
        break;

      case ProgramPoint::PostStmtKind:
        HandlePostStmt(cast<PostStmt>(Node->getLocation()), WU.getBlock(),
                       WU.getIndex(), Node);
        break;
    }
  }

  return WList->hasWork();
}

void GRCoreEngineImpl::HandleBlockEdge(const BlockEdge& L,
                                       ExplodedNodeImpl* Pred) {

  CFGBlock* Blk = L.getDst();

  // Check if we are entering the EXIT block.
  if (Blk == &getCFG().getExit()) {

    assert (getCFG().getExit().size() == 0
            && "EXIT block cannot contain Stmts.");

    // Process the final state transition.
    GREndPathNodeBuilderImpl Builder(Blk, Pred, this);
    ProcessEndPath(Builder);

    // This path is done. Don't enqueue any more nodes.
    return;
  }

  // FIXME: Should we allow ProcessBlockEntrance to also manipulate state?

  if (ProcessBlockEntrance(Blk, Pred->State, WList->getBlockCounter()))
    GenerateNode(BlockEntrance(Blk), Pred->State, Pred);
}

void GRCoreEngineImpl::HandleBlockEntrance(const BlockEntrance& L,
                                           ExplodedNodeImpl* Pred) {

  // Increment the block counter.
  GRBlockCounter Counter = WList->getBlockCounter();
  Counter = BCounterFactory.IncrementCount(Counter, L.getBlock()->getBlockID());
  WList->setBlockCounter(Counter);

  // Process the entrance of the block.
  if (Stmt* S = L.getFirstStmt()) {
    GRStmtNodeBuilderImpl Builder(L.getBlock(), 0, Pred, this);
    ProcessStmt(S, Builder);
  }
  else
    HandleBlockExit(L.getBlock(), Pred);
}

void GRCoreEngineImpl::HandleBlockExit(CFGBlock * B, ExplodedNodeImpl* Pred) {

  if (Stmt* Term = B->getTerminator()) {
    switch (Term->getStmtClass()) {
      default:
        assert(false && "Analysis for this terminator not implemented.");
        break;

      case Stmt::BinaryOperatorClass: // '&&' and '||'
        HandleBranch(cast<BinaryOperator>(Term)->getLHS(), Term, B, Pred);
        return;

      case Stmt::ConditionalOperatorClass:
        HandleBranch(cast<ConditionalOperator>(Term)->getCond(), Term, B, Pred);
        return;

        // FIXME: Use constant-folding in CFG construction to simplify this
        // case.

      case Stmt::ChooseExprClass:
        HandleBranch(cast<ChooseExpr>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::DoStmtClass:
        HandleBranch(cast<DoStmt>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::ForStmtClass:
        HandleBranch(cast<ForStmt>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::ContinueStmtClass:
      case Stmt::BreakStmtClass:
      case Stmt::GotoStmtClass:
        break;

      case Stmt::IfStmtClass:
        HandleBranch(cast<IfStmt>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::IndirectGotoStmtClass: {
        // Only 1 successor: the indirect goto dispatch block.
        assert (B->succ_size() == 1);

        GRIndirectGotoNodeBuilderImpl
           builder(Pred, B, cast<IndirectGotoStmt>(Term)->getTarget(),
                   *(B->succ_begin()), this);

        ProcessIndirectGoto(builder);
        return;
      }

      case Stmt::SwitchStmtClass: {
        GRSwitchNodeBuilderImpl builder(Pred, B,
                                        cast<SwitchStmt>(Term)->getCond(),
                                        this);

        ProcessSwitch(builder);
        return;
      }

      case Stmt::WhileStmtClass:
        HandleBranch(cast<WhileStmt>(Term)->getCond(), Term, B, Pred);
        return;
    }
  }

  assert (B->succ_size() == 1 &&
          "Blocks with no terminator should have at most 1 successor.");

  GenerateNode(BlockEdge(getCFG(),B,*(B->succ_begin())), Pred->State, Pred);
}

void GRCoreEngineImpl::HandleBranch(Expr* Cond, Stmt* Term, CFGBlock * B,
                                ExplodedNodeImpl* Pred) {
  assert (B->succ_size() == 2);

  GRBranchNodeBuilderImpl Builder(B, *(B->succ_begin()), *(B->succ_begin()+1),
                                  Pred, this);

  ProcessBranch(Cond, Term, Builder);
}

void GRCoreEngineImpl::HandlePostStmt(const PostStmt& L, CFGBlock* B,
                                  unsigned StmtIdx, ExplodedNodeImpl* Pred) {

  assert (!B->empty());

  if (StmtIdx == B->size())
    HandleBlockExit(B, Pred);
  else {
    GRStmtNodeBuilderImpl Builder(B, StmtIdx, Pred, this);
    ProcessStmt((*B)[StmtIdx], Builder);
  }
}

typedef llvm::DenseMap<Stmt*,Stmt*> ParentMapTy;
/// PopulateParentMap - Recurse the AST starting at 'Parent' and add the
///  mappings between child and parent to ParentMap.
static void PopulateParentMap(Stmt* Parent, ParentMapTy& M) {
  for (Stmt::child_iterator I=Parent->child_begin(),
       E=Parent->child_end(); I!=E; ++I) {

    assert (M.find(*I) == M.end());
    M[*I] = Parent;
    PopulateParentMap(*I, M);
  }
}

/// GenerateNode - Utility method to generate nodes, hook up successors,
///  and add nodes to the worklist.
void GRCoreEngineImpl::GenerateNode(const ProgramPoint& Loc, void* State,
                                ExplodedNodeImpl* Pred) {

  bool IsNew;
  ExplodedNodeImpl* Node = G->getNodeImpl(Loc, State, &IsNew);

  if (Pred)
    Node->addPredecessor(Pred);  // Link 'Node' with its predecessor.
  else {
    assert (IsNew);
    G->addRoot(Node);  // 'Node' has no predecessor.  Make it a root.
  }

  // Only add 'Node' to the worklist if it was freshly generated.
  if (IsNew) WList->Enqueue(Node);
}

GRStmtNodeBuilderImpl::GRStmtNodeBuilderImpl(CFGBlock* b, unsigned idx,
                                     ExplodedNodeImpl* N, GRCoreEngineImpl* e)
  : Eng(*e), B(*b), Idx(idx), Pred(N), LastNode(N), Populated(false) {
  Deferred.insert(N);
}

GRStmtNodeBuilderImpl::~GRStmtNodeBuilderImpl() {
  for (DeferredTy::iterator I=Deferred.begin(), E=Deferred.end(); I!=E; ++I)
    if (!(*I)->isSink())
      GenerateAutoTransition(*I);
}

void GRStmtNodeBuilderImpl::GenerateAutoTransition(ExplodedNodeImpl* N) {
  assert (!N->isSink());

  PostStmt Loc(getStmt());

  if (Loc == N->getLocation()) {
    // Note: 'N' should be a fresh node because otherwise it shouldn't be
    // a member of Deferred.
    Eng.WList->Enqueue(N, B, Idx+1);
    return;
  }

  bool IsNew;
  ExplodedNodeImpl* Succ = Eng.G->getNodeImpl(Loc, N->State, &IsNew);
  Succ->addPredecessor(N);

  if (IsNew)
    Eng.WList->Enqueue(Succ, B, Idx+1);
}

ExplodedNodeImpl* GRStmtNodeBuilderImpl::generateNodeImpl(Stmt* S, void* State,
                                                      ExplodedNodeImpl* Pred) {

  bool IsNew;
  ExplodedNodeImpl* N = Eng.G->getNodeImpl(PostStmt(S), State, &IsNew);
  N->addPredecessor(Pred);
  Deferred.erase(Pred);

  HasGeneratedNode = true;

  if (IsNew) {
    Deferred.insert(N);
    LastNode = N;
    return N;
  }

  LastNode = NULL;
  return NULL;
}

ExplodedNodeImpl* GRBranchNodeBuilderImpl::generateNodeImpl(void* State,
                                                            bool branch) {
  bool IsNew;

  ExplodedNodeImpl* Succ =
    Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src, branch ? DstT : DstF),
                       State, &IsNew);

  Succ->addPredecessor(Pred);

  if (branch) GeneratedTrue = true;
  else GeneratedFalse = true;

  if (IsNew) {
    Deferred.push_back(Succ);
    return Succ;
  }

  return NULL;
}

GRBranchNodeBuilderImpl::~GRBranchNodeBuilderImpl() {
  if (!GeneratedTrue) generateNodeImpl(Pred->State, true);
  if (!GeneratedFalse) generateNodeImpl(Pred->State, false);

  for (DeferredTy::iterator I=Deferred.begin(), E=Deferred.end(); I!=E; ++I)
    if (!(*I)->isSink()) Eng.WList->Enqueue(*I);
}

ExplodedNodeImpl*
GRIndirectGotoNodeBuilderImpl::generateNodeImpl(const Iterator& I,
                                                void* St,
                                                bool isSink) {
  bool IsNew;

  ExplodedNodeImpl* Succ =
    Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src, I.getBlock(), true),
                       St, &IsNew);

  Succ->addPredecessor(Pred);

  if (IsNew) {

    if (isSink)
      Succ->markAsSink();
    else
      Eng.WList->Enqueue(Succ);

    return Succ;
  }

  return NULL;
}

ExplodedNodeImpl*
GRSwitchNodeBuilderImpl::generateCaseStmtNodeImpl(const Iterator& I, void* St) {

  bool IsNew;

  ExplodedNodeImpl* Succ = Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src,
                                                        I.getBlock()),
                                              St, &IsNew);
  Succ->addPredecessor(Pred);

  if (IsNew) {
    Eng.WList->Enqueue(Succ);
    return Succ;
  }

  return NULL;
}

ExplodedNodeImpl*
GRSwitchNodeBuilderImpl::generateDefaultCaseNodeImpl(void* St, bool isSink) {

  // Get the block for the default case.
  assert (Src->succ_rbegin() != Src->succ_rend());
  CFGBlock* DefaultBlock = *Src->succ_rbegin();

  bool IsNew;

  ExplodedNodeImpl* Succ = Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src,
                                                        DefaultBlock),
                                              St, &IsNew);
  Succ->addPredecessor(Pred);

  if (IsNew) {
    if (isSink)
      Succ->markAsSink();
    else
      Eng.WList->Enqueue(Succ);

    return Succ;
  }

  return NULL;
}

GREndPathNodeBuilderImpl::~GREndPathNodeBuilderImpl() {
  // Auto-generate an EOP node if one has not been generated.
  if (!HasGeneratedNode) generateNodeImpl(Pred->State);
}

ExplodedNodeImpl* GREndPathNodeBuilderImpl::generateNodeImpl(void* State) {
  HasGeneratedNode = true;

  bool IsNew;

  ExplodedNodeImpl* Node =
    Eng.G->getNodeImpl(BlockEntrance(&B), Pred->State, &IsNew);

  Node->addPredecessor(Pred);

  if (IsNew) {
    Node->markAsSink();
    Eng.G->addEndOfPath(Node);
  }

  return Node;
}
<commit_msg>Bug fix in GREndPathNodeBuilderImpl: Use the specified state to construct a node, not the state of the predecessor.<commit_after>//==- GRCoreEngine.cpp - Path-Sensitive Dataflow Engine ----------------*- C++ -*-//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
//  This file defines a generic engine for intraprocedural, path-sensitive,
//  dataflow analysis via graph reachability engine.
//
//===----------------------------------------------------------------------===//

#include "clang/Analysis/PathSensitive/GRCoreEngine.h"
#include "clang/AST/Expr.h"
#include "llvm/Support/Compiler.h"
#include "llvm/Support/Casting.h"
#include "llvm/ADT/DenseMap.h"
#include <vector>

using llvm::cast;
using llvm::isa;
using namespace clang;

namespace {
  class VISIBILITY_HIDDEN DFS : public GRWorkList {
  llvm::SmallVector<GRWorkListUnit,20> Stack;
public:
  virtual bool hasWork() const {
    return !Stack.empty();
  }

  virtual void Enqueue(const GRWorkListUnit& U) {
    Stack.push_back(U);
  }

  virtual GRWorkListUnit Dequeue() {
    assert (!Stack.empty());
    const GRWorkListUnit& U = Stack.back();
    Stack.pop_back(); // This technically "invalidates" U, but we are fine.
    return U;
  }
};
} // end anonymous namespace

// Place the dstor for GRWorkList here because it contains virtual member
// functions, and we the code for the dstor generated in one compilation unit.
GRWorkList::~GRWorkList() {}

GRWorkList* GRWorkList::MakeDFS() { return new DFS(); }

/// ExecuteWorkList - Run the worklist algorithm for a maximum number of steps.
bool GRCoreEngineImpl::ExecuteWorkList(unsigned Steps) {

  if (G->num_roots() == 0) { // Initialize the analysis by constructing
    // the root if none exists.

    CFGBlock* Entry = &getCFG().getEntry();

    assert (Entry->empty() &&
            "Entry block must be empty.");

    assert (Entry->succ_size() == 1 &&
            "Entry block must have 1 successor.");

    // Get the solitary successor.
    CFGBlock* Succ = *(Entry->succ_begin());

    // Construct an edge representing the
    // starting location in the function.
    BlockEdge StartLoc(getCFG(), Entry, Succ);

    // Set the current block counter to being empty.
    WList->setBlockCounter(BCounterFactory.GetEmptyCounter());

    // Generate the root.
    GenerateNode(StartLoc, getInitialState());
  }

  while (Steps && WList->hasWork()) {
    --Steps;
    const GRWorkListUnit& WU = WList->Dequeue();

    // Set the current block counter.
    WList->setBlockCounter(WU.getBlockCounter());

    // Retrieve the node.
    ExplodedNodeImpl* Node = WU.getNode();

    // Dispatch on the location type.
    switch (Node->getLocation().getKind()) {
      default:
        assert (isa<BlockEdge>(Node->getLocation()));
        HandleBlockEdge(cast<BlockEdge>(Node->getLocation()), Node);
        break;

      case ProgramPoint::BlockEntranceKind:
        HandleBlockEntrance(cast<BlockEntrance>(Node->getLocation()), Node);
        break;

      case ProgramPoint::BlockExitKind:
        assert (false && "BlockExit location never occur in forward analysis.");
        break;

      case ProgramPoint::PostStmtKind:
        HandlePostStmt(cast<PostStmt>(Node->getLocation()), WU.getBlock(),
                       WU.getIndex(), Node);
        break;
    }
  }

  return WList->hasWork();
}

void GRCoreEngineImpl::HandleBlockEdge(const BlockEdge& L,
                                       ExplodedNodeImpl* Pred) {

  CFGBlock* Blk = L.getDst();

  // Check if we are entering the EXIT block.
  if (Blk == &getCFG().getExit()) {

    assert (getCFG().getExit().size() == 0
            && "EXIT block cannot contain Stmts.");

    // Process the final state transition.
    GREndPathNodeBuilderImpl Builder(Blk, Pred, this);
    ProcessEndPath(Builder);

    // This path is done. Don't enqueue any more nodes.
    return;
  }

  // FIXME: Should we allow ProcessBlockEntrance to also manipulate state?

  if (ProcessBlockEntrance(Blk, Pred->State, WList->getBlockCounter()))
    GenerateNode(BlockEntrance(Blk), Pred->State, Pred);
}

void GRCoreEngineImpl::HandleBlockEntrance(const BlockEntrance& L,
                                           ExplodedNodeImpl* Pred) {

  // Increment the block counter.
  GRBlockCounter Counter = WList->getBlockCounter();
  Counter = BCounterFactory.IncrementCount(Counter, L.getBlock()->getBlockID());
  WList->setBlockCounter(Counter);

  // Process the entrance of the block.
  if (Stmt* S = L.getFirstStmt()) {
    GRStmtNodeBuilderImpl Builder(L.getBlock(), 0, Pred, this);
    ProcessStmt(S, Builder);
  }
  else
    HandleBlockExit(L.getBlock(), Pred);
}

void GRCoreEngineImpl::HandleBlockExit(CFGBlock * B, ExplodedNodeImpl* Pred) {

  if (Stmt* Term = B->getTerminator()) {
    switch (Term->getStmtClass()) {
      default:
        assert(false && "Analysis for this terminator not implemented.");
        break;

      case Stmt::BinaryOperatorClass: // '&&' and '||'
        HandleBranch(cast<BinaryOperator>(Term)->getLHS(), Term, B, Pred);
        return;

      case Stmt::ConditionalOperatorClass:
        HandleBranch(cast<ConditionalOperator>(Term)->getCond(), Term, B, Pred);
        return;

        // FIXME: Use constant-folding in CFG construction to simplify this
        // case.

      case Stmt::ChooseExprClass:
        HandleBranch(cast<ChooseExpr>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::DoStmtClass:
        HandleBranch(cast<DoStmt>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::ForStmtClass:
        HandleBranch(cast<ForStmt>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::ContinueStmtClass:
      case Stmt::BreakStmtClass:
      case Stmt::GotoStmtClass:
        break;

      case Stmt::IfStmtClass:
        HandleBranch(cast<IfStmt>(Term)->getCond(), Term, B, Pred);
        return;

      case Stmt::IndirectGotoStmtClass: {
        // Only 1 successor: the indirect goto dispatch block.
        assert (B->succ_size() == 1);

        GRIndirectGotoNodeBuilderImpl
           builder(Pred, B, cast<IndirectGotoStmt>(Term)->getTarget(),
                   *(B->succ_begin()), this);

        ProcessIndirectGoto(builder);
        return;
      }

      case Stmt::SwitchStmtClass: {
        GRSwitchNodeBuilderImpl builder(Pred, B,
                                        cast<SwitchStmt>(Term)->getCond(),
                                        this);

        ProcessSwitch(builder);
        return;
      }

      case Stmt::WhileStmtClass:
        HandleBranch(cast<WhileStmt>(Term)->getCond(), Term, B, Pred);
        return;
    }
  }

  assert (B->succ_size() == 1 &&
          "Blocks with no terminator should have at most 1 successor.");

  GenerateNode(BlockEdge(getCFG(),B,*(B->succ_begin())), Pred->State, Pred);
}

void GRCoreEngineImpl::HandleBranch(Expr* Cond, Stmt* Term, CFGBlock * B,
                                ExplodedNodeImpl* Pred) {
  assert (B->succ_size() == 2);

  GRBranchNodeBuilderImpl Builder(B, *(B->succ_begin()), *(B->succ_begin()+1),
                                  Pred, this);

  ProcessBranch(Cond, Term, Builder);
}

void GRCoreEngineImpl::HandlePostStmt(const PostStmt& L, CFGBlock* B,
                                  unsigned StmtIdx, ExplodedNodeImpl* Pred) {

  assert (!B->empty());

  if (StmtIdx == B->size())
    HandleBlockExit(B, Pred);
  else {
    GRStmtNodeBuilderImpl Builder(B, StmtIdx, Pred, this);
    ProcessStmt((*B)[StmtIdx], Builder);
  }
}

typedef llvm::DenseMap<Stmt*,Stmt*> ParentMapTy;
/// PopulateParentMap - Recurse the AST starting at 'Parent' and add the
///  mappings between child and parent to ParentMap.
static void PopulateParentMap(Stmt* Parent, ParentMapTy& M) {
  for (Stmt::child_iterator I=Parent->child_begin(),
       E=Parent->child_end(); I!=E; ++I) {

    assert (M.find(*I) == M.end());
    M[*I] = Parent;
    PopulateParentMap(*I, M);
  }
}

/// GenerateNode - Utility method to generate nodes, hook up successors,
///  and add nodes to the worklist.
void GRCoreEngineImpl::GenerateNode(const ProgramPoint& Loc, void* State,
                                ExplodedNodeImpl* Pred) {

  bool IsNew;
  ExplodedNodeImpl* Node = G->getNodeImpl(Loc, State, &IsNew);

  if (Pred)
    Node->addPredecessor(Pred);  // Link 'Node' with its predecessor.
  else {
    assert (IsNew);
    G->addRoot(Node);  // 'Node' has no predecessor.  Make it a root.
  }

  // Only add 'Node' to the worklist if it was freshly generated.
  if (IsNew) WList->Enqueue(Node);
}

GRStmtNodeBuilderImpl::GRStmtNodeBuilderImpl(CFGBlock* b, unsigned idx,
                                     ExplodedNodeImpl* N, GRCoreEngineImpl* e)
  : Eng(*e), B(*b), Idx(idx), Pred(N), LastNode(N), Populated(false) {
  Deferred.insert(N);
}

GRStmtNodeBuilderImpl::~GRStmtNodeBuilderImpl() {
  for (DeferredTy::iterator I=Deferred.begin(), E=Deferred.end(); I!=E; ++I)
    if (!(*I)->isSink())
      GenerateAutoTransition(*I);
}

void GRStmtNodeBuilderImpl::GenerateAutoTransition(ExplodedNodeImpl* N) {
  assert (!N->isSink());

  PostStmt Loc(getStmt());

  if (Loc == N->getLocation()) {
    // Note: 'N' should be a fresh node because otherwise it shouldn't be
    // a member of Deferred.
    Eng.WList->Enqueue(N, B, Idx+1);
    return;
  }

  bool IsNew;
  ExplodedNodeImpl* Succ = Eng.G->getNodeImpl(Loc, N->State, &IsNew);
  Succ->addPredecessor(N);

  if (IsNew)
    Eng.WList->Enqueue(Succ, B, Idx+1);
}

ExplodedNodeImpl* GRStmtNodeBuilderImpl::generateNodeImpl(Stmt* S, void* State,
                                                      ExplodedNodeImpl* Pred) {

  bool IsNew;
  ExplodedNodeImpl* N = Eng.G->getNodeImpl(PostStmt(S), State, &IsNew);
  N->addPredecessor(Pred);
  Deferred.erase(Pred);

  HasGeneratedNode = true;

  if (IsNew) {
    Deferred.insert(N);
    LastNode = N;
    return N;
  }

  LastNode = NULL;
  return NULL;
}

ExplodedNodeImpl* GRBranchNodeBuilderImpl::generateNodeImpl(void* State,
                                                            bool branch) {
  bool IsNew;

  ExplodedNodeImpl* Succ =
    Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src, branch ? DstT : DstF),
                       State, &IsNew);

  Succ->addPredecessor(Pred);

  if (branch) GeneratedTrue = true;
  else GeneratedFalse = true;

  if (IsNew) {
    Deferred.push_back(Succ);
    return Succ;
  }

  return NULL;
}

GRBranchNodeBuilderImpl::~GRBranchNodeBuilderImpl() {
  if (!GeneratedTrue) generateNodeImpl(Pred->State, true);
  if (!GeneratedFalse) generateNodeImpl(Pred->State, false);

  for (DeferredTy::iterator I=Deferred.begin(), E=Deferred.end(); I!=E; ++I)
    if (!(*I)->isSink()) Eng.WList->Enqueue(*I);
}

ExplodedNodeImpl*
GRIndirectGotoNodeBuilderImpl::generateNodeImpl(const Iterator& I,
                                                void* St,
                                                bool isSink) {
  bool IsNew;

  ExplodedNodeImpl* Succ =
    Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src, I.getBlock(), true),
                       St, &IsNew);

  Succ->addPredecessor(Pred);

  if (IsNew) {

    if (isSink)
      Succ->markAsSink();
    else
      Eng.WList->Enqueue(Succ);

    return Succ;
  }

  return NULL;
}

ExplodedNodeImpl*
GRSwitchNodeBuilderImpl::generateCaseStmtNodeImpl(const Iterator& I, void* St) {

  bool IsNew;

  ExplodedNodeImpl* Succ = Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src,
                                                        I.getBlock()),
                                              St, &IsNew);
  Succ->addPredecessor(Pred);

  if (IsNew) {
    Eng.WList->Enqueue(Succ);
    return Succ;
  }

  return NULL;
}

ExplodedNodeImpl*
GRSwitchNodeBuilderImpl::generateDefaultCaseNodeImpl(void* St, bool isSink) {

  // Get the block for the default case.
  assert (Src->succ_rbegin() != Src->succ_rend());
  CFGBlock* DefaultBlock = *Src->succ_rbegin();

  bool IsNew;

  ExplodedNodeImpl* Succ = Eng.G->getNodeImpl(BlockEdge(Eng.getCFG(), Src,
                                                        DefaultBlock),
                                              St, &IsNew);
  Succ->addPredecessor(Pred);

  if (IsNew) {
    if (isSink)
      Succ->markAsSink();
    else
      Eng.WList->Enqueue(Succ);

    return Succ;
  }

  return NULL;
}

GREndPathNodeBuilderImpl::~GREndPathNodeBuilderImpl() {
  // Auto-generate an EOP node if one has not been generated.
  if (!HasGeneratedNode) generateNodeImpl(Pred->State);
}

ExplodedNodeImpl* GREndPathNodeBuilderImpl::generateNodeImpl(void* State) {
  HasGeneratedNode = true;

  bool IsNew;

  ExplodedNodeImpl* Node =
    Eng.G->getNodeImpl(BlockEntrance(&B), State, &IsNew);

  Node->addPredecessor(Pred);

  if (IsNew) {
    Node->markAsSink();
    Eng.G->addEndOfPath(Node);
  }

  return Node;
}
<|endoftext|>