<commit_before>// Copyright 2010-2011 Google
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "base/callback.h"
#include "base/integral_types.h"
#include "base/logging.h"
#include "base/scoped_ptr.h"
#include "base/stringprintf.h"
#include "base/concise_iterator.h"
#include "base/stl_util-inl.h"
#include "constraint_solver/constraint_solveri.h"

namespace operations_research {

// ----- Base Class -----

void NoGoodManager::EnterSearch() {
  Init();
}

void NoGoodManager::BeginNextDecision(DecisionBuilder* const db) {
  Apply();
}

bool NoGoodManager::AcceptSolution() {
  Apply();
  return true;
}

NoGood* NoGoodManager::MakeNoGood() {
  return new NoGood();
}

// ----- Base class for NoGood terms -----

class NoGoodTerm {
 public:
  enum TermStatus {
    ALWAYS_TRUE,
    ALWAYS_FALSE,
    UNDECIDED
  };
  NoGoodTerm() {}
  virtual ~NoGoodTerm() {}

  virtual TermStatus Evaluate() const = 0;
  virtual void ApplyReverse() = 0;
  virtual string DebugString() const = 0;
 private:
  DISALLOW_COPY_AND_ASSIGN(NoGoodTerm);
};

// ----- IntegerVariableNoGoodTerm -----

// IntVar == int64 specialization.
class IntegerVariableNoGoodTerm : public NoGoodTerm {
 public:
  IntegerVariableNoGoodTerm(IntVar* const var, int64 value, bool assign)
      : integer_variable_(var), value_(value), assign_(assign) {
    CHECK_NOTNULL(integer_variable_);
  }

  virtual TermStatus Evaluate() const {
    if (!integer_variable_->Contains(value_)) {
      return assign_ ? ALWAYS_FALSE : ALWAYS_TRUE;
    } else if (integer_variable_->Bound()) {
      return assign_ ? ALWAYS_TRUE : ALWAYS_FALSE;
    } else {
      return UNDECIDED;
    }
  }

  virtual void ApplyReverse() {
    if (assign_) {
      integer_variable_->RemoveValue(value_);
    } else {
      integer_variable_->SetValue(value_);
    }
  }

  virtual string DebugString() const {
    return StringPrintf("(%s %s %lld)",
                        integer_variable_->name().c_str(),
                        assign_ ? "==" : "!=",
                        value_);
  }

  IntVar* integer_variable() const { return integer_variable_; }
  int64 value() const { return value_; }
  bool assign() const { return assign_; }

 private:
  IntVar* const integer_variable_;
  const int64 value_;
  const bool assign_;
  DISALLOW_COPY_AND_ASSIGN(IntegerVariableNoGoodTerm);
};

// ----- NoGood -----

NoGood::~NoGood() {
  STLDeleteElements(&terms_);
}

void NoGood::AddIntegerVariableEqualValueTerm(IntVar* const var, int64 value) {
  terms_.push_back(new IntegerVariableNoGoodTerm(var, value, true));
}

void NoGood::AddIntegerVariableNotEqualValueTerm(IntVar* const var,
                                                 int64 value) {
  terms_.push_back(new IntegerVariableNoGoodTerm(var, value, false));
}

void NoGood::Apply(Solver* const solver) {
  NoGoodTerm* first_undecided = NULL;
  for (int i = 0; i < terms_.size(); ++i) {
    switch (terms_[i]->Evaluate()) {
      case NoGoodTerm::ALWAYS_TRUE: {
        break;
      }
      case NoGoodTerm::ALWAYS_FALSE: {
        return;
      }
      case NoGoodTerm::UNDECIDED: {
        if (first_undecided == NULL) {
          first_undecided = terms_[i];
        } else {
          // more than one undecided, we cannot deduce anything.
          return;
        }
        break;
      }
    }
  }
  if (first_undecided == NULL && terms_.size() > 0) {
    solver->Fail();
  }
  if (first_undecided != NULL) {
    first_undecided->ApplyReverse();
  }
}

string NoGood::DebugString() const {
  string result = "(";
  for (int i = 0; i < terms_.size(); ++i) {
    if (i != 0) {
      result.append(" && ");
    }
    result.append(terms_[i]->DebugString());
  }
  result.append(")");
  return result;
}

// ----- NoGoodManager -----

// This implementation is very naive. It will be kept in the future as
// a reference point.

class NaiveNoGoodManager : public NoGoodManager {
 public:
  explicit NaiveNoGoodManager(Solver* const solver) : NoGoodManager(solver) {}
  virtual ~NaiveNoGoodManager() {
    Clear();
  }

  virtual void Clear() {
    STLDeleteElements(&nogoods_);
  }

  virtual void Init() {}

  virtual void AddNoGood(NoGood* const nogood) {
    nogoods_.push_back(nogood);
  }

  virtual int NoGoodCount() const {
    return nogoods_.size();
  }

  virtual void Apply() {
    Solver* const s = solver();
    for (int i = 0; i < nogoods_.size(); ++i) {
      nogoods_[i]->Apply(s);
    }
  }

  string DebugString() const {
    return StringPrintf("NaiveNoGoodManager(%d)", NoGoodCount());
  }

 private:
  std::vector<NoGood*> nogoods_;
};

// ----- API -----

NoGoodManager* Solver::MakeNoGoodManager() {
  return RevAlloc(new NaiveNoGoodManager(this));
}

}  // namespace operations_research
<commit_msg>rename ApplyReverse to Refute<commit_after>// Copyright 2010-2011 Google
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "base/callback.h"
#include "base/integral_types.h"
#include "base/logging.h"
#include "base/scoped_ptr.h"
#include "base/stringprintf.h"
#include "base/concise_iterator.h"
#include "base/stl_util-inl.h"
#include "constraint_solver/constraint_solveri.h"

namespace operations_research {

// ----- Base Class -----

void NoGoodManager::EnterSearch() {
  Init();
}

void NoGoodManager::BeginNextDecision(DecisionBuilder* const db) {
  Apply();
}

bool NoGoodManager::AcceptSolution() {
  Apply();
  return true;
}

NoGood* NoGoodManager::MakeNoGood() {
  return new NoGood();
}

// ----- Base class for NoGood terms -----

class NoGoodTerm {
 public:
  enum TermStatus {
    ALWAYS_TRUE,
    ALWAYS_FALSE,
    UNDECIDED
  };
  NoGoodTerm() {}
  virtual ~NoGoodTerm() {}

  virtual TermStatus Evaluate() const = 0;
  virtual void Refute() = 0;
  virtual string DebugString() const = 0;
 private:
  DISALLOW_COPY_AND_ASSIGN(NoGoodTerm);
};

// ----- IntegerVariableNoGoodTerm -----

// IntVar == int64 specialization.
class IntegerVariableNoGoodTerm : public NoGoodTerm {
 public:
  IntegerVariableNoGoodTerm(IntVar* const var, int64 value, bool assign)
      : integer_variable_(var), value_(value), assign_(assign) {
    CHECK_NOTNULL(integer_variable_);
  }

  virtual TermStatus Evaluate() const {
    if (!integer_variable_->Contains(value_)) {
      return assign_ ? ALWAYS_FALSE : ALWAYS_TRUE;
    } else if (integer_variable_->Bound()) {
      return assign_ ? ALWAYS_TRUE : ALWAYS_FALSE;
    } else {
      return UNDECIDED;
    }
  }

  virtual void Refute() {
    if (assign_) {
      integer_variable_->RemoveValue(value_);
    } else {
      integer_variable_->SetValue(value_);
    }
  }

  virtual string DebugString() const {
    return StringPrintf("(%s %s %lld)",
                        integer_variable_->name().c_str(),
                        assign_ ? "==" : "!=",
                        value_);
  }

  IntVar* integer_variable() const { return integer_variable_; }
  int64 value() const { return value_; }
  bool assign() const { return assign_; }

 private:
  IntVar* const integer_variable_;
  const int64 value_;
  const bool assign_;
  DISALLOW_COPY_AND_ASSIGN(IntegerVariableNoGoodTerm);
};

// ----- NoGood -----

NoGood::~NoGood() {
  STLDeleteElements(&terms_);
}

void NoGood::AddIntegerVariableEqualValueTerm(IntVar* const var, int64 value) {
  terms_.push_back(new IntegerVariableNoGoodTerm(var, value, true));
}

void NoGood::AddIntegerVariableNotEqualValueTerm(IntVar* const var,
                                                 int64 value) {
  terms_.push_back(new IntegerVariableNoGoodTerm(var, value, false));
}

void NoGood::Apply(Solver* const solver) {
  NoGoodTerm* first_undecided = NULL;
  for (int i = 0; i < terms_.size(); ++i) {
    switch (terms_[i]->Evaluate()) {
      case NoGoodTerm::ALWAYS_TRUE: {
        break;
      }
      case NoGoodTerm::ALWAYS_FALSE: {
        return;
      }
      case NoGoodTerm::UNDECIDED: {
        if (first_undecided == NULL) {
          first_undecided = terms_[i];
        } else {
          // more than one undecided, we cannot deduce anything.
          return;
        }
        break;
      }
    }
  }
  if (first_undecided == NULL && terms_.size() > 0) {
    solver->Fail();
  }
  if (first_undecided != NULL) {
    first_undecided->Refute();
  }
}

string NoGood::DebugString() const {
  string result = "(";
  for (int i = 0; i < terms_.size(); ++i) {
    if (i != 0) {
      result.append(" && ");
    }
    result.append(terms_[i]->DebugString());
  }
  result.append(")");
  return result;
}

// ----- NoGoodManager -----

// This implementation is very naive. It will be kept in the future as
// a reference point.

class NaiveNoGoodManager : public NoGoodManager {
 public:
  explicit NaiveNoGoodManager(Solver* const solver) : NoGoodManager(solver) {}
  virtual ~NaiveNoGoodManager() {
    Clear();
  }

  virtual void Clear() {
    STLDeleteElements(&nogoods_);
  }

  virtual void Init() {}

  virtual void AddNoGood(NoGood* const nogood) {
    nogoods_.push_back(nogood);
  }

  virtual int NoGoodCount() const {
    return nogoods_.size();
  }

  virtual void Apply() {
    Solver* const s = solver();
    for (int i = 0; i < nogoods_.size(); ++i) {
      nogoods_[i]->Apply(s);
    }
  }

  string DebugString() const {
    return StringPrintf("NaiveNoGoodManager(%d)", NoGoodCount());
  }

 private:
  std::vector<NoGood*> nogoods_;
};

// ----- API -----

NoGoodManager* Solver::MakeNoGoodManager() {
  return RevAlloc(new NaiveNoGoodManager(this));
}

}  // namespace operations_research
<|endoftext|>


<commit_before>/*=========================================================================

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

=========================================================================*/

#include <irtkImage.h>

#include <irtkTransformation.h>

// Default filenames
char *input_name = NULL, *output_name = NULL, *dof_name  = NULL;

void usage()
{
  cerr << "Usage: transformation [source] [output] <options>\n" << endl;
  cerr << "where <options> is one or more of the following:\n" << endl;
  cerr << "<-dofin file>      Transformation" << endl;
  cerr << "<-target file>     Target image" << endl;
  cerr << "<-Rx1 value>       Region of interest" << endl;
  cerr << "<-Ry1 value>       Region of interest" << endl;
  cerr << "<-Rz1 value>       Region of interest" << endl;
  cerr << "<-Rx2 value>       Region of interest" << endl;
  cerr << "<-Ry2 value>       Region of interest" << endl;
  cerr << "<-Rz2 value>       Region of interest" << endl;
  cerr << "<-Tx1 value>       Region of interest in target image" << endl;
  cerr << "<-Ty1 value>       Region of interest in target image" << endl;
  cerr << "<-Tz1 value>       Region of interest in target image" << endl;
  cerr << "<-Tx2 value>       Region of interest in target image" << endl;
  cerr << "<-Ty2 value>       Region of interest in target image" << endl;
  cerr << "<-Tz2 value>       Region of interest in target image" << endl;
  cerr << "<-Sx1 value>       Region of interest in source image" << endl;
  cerr << "<-Sy1 value>       Region of interest in source image" << endl;
  cerr << "<-Sz1 value>       Region of interest in source image" << endl;
  cerr << "<-Sx2 value>       Region of interest in source image" << endl;
  cerr << "<-Sy2 value>       Region of interest in source image" << endl;
  cerr << "<-Sz2 value>       Region of interest in source image" << endl;
  cerr << "<-Tp value>        Target padding value" << endl;
  cerr << "<-Sp value>        Source padding value" << endl;
  cerr << "<-invert>          Invert transformation" << endl;
  cerr << "<-linear>          Linear interpolation" << endl;
  cerr << "<-bspline>         B-spline interpolation" << endl;
  cerr << "<-cspline>         Cubic spline interpolation" << endl;
  cerr << "<-sinc>            Sinc interpolation" << endl;
  exit(1);
}

int main(int argc, char **argv)
{
  int ok, invert;
  int target_padding, source_padding;
  int target_x1, target_y1, target_z1, target_x2, target_y2, target_z2;
  int source_x1, source_y1, source_z1, source_x2, source_y2, source_z2;
  irtkTransformation *transformation = NULL;
  irtkGreyImage *source = NULL;
  irtkGreyImage *target = NULL;
  irtkImageFunction *interpolator = NULL;

  // Check command line
  if (argc < 3) {
    usage();
  }

  // Parse image
  input_name  = argv[1];
  argc--;
  argv++;
  output_name = argv[1];
  argc--;
  argv++;

  // Read image
  cout << "Reading image ... "; cout.flush();
  source = new irtkGreyImage(input_name);
  cout << "done" << endl;

  // Fix ROI
  target_x1 = -1;
  target_y1 = -1;
  target_z1 = -1;
  target_x2 = -1;
  target_y2 = -1;
  target_z2 = -1;
  source_x1 = -1;
  source_y1 = -1;
  source_z1 = -1;
  source_x2 = -1;
  source_y2 = -1;
  source_z2 = -1;

  // Other options
  invert = False;
  source_padding = 0;
  target_padding = MIN_GREY;

  while (argc > 1) {
    ok = False;
    if ((ok == False) && (strcmp(argv[1], "-dofin") == 0)) {
      argc--;
      argv++;
      dof_name = argv[1];
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-target") == 0)) {
      argc--;
      argv++;
      target = new irtkGreyImage(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rx1") == 0)) {
      argc--;
      argv++;
      target_x1 = source_x1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rx2") == 0)) {
      argc--;
      argv++;
      target_x2 = source_x2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ry1") == 0)) {
      argc--;
      argv++;
      target_y1 = source_y1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ry2") == 0)) {
      argc--;
      argv++;
      target_y2 = source_y2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rz1") == 0)) {
      argc--;
      argv++;
      target_z1 = source_z1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rz2") == 0)) {
      argc--;
      argv++;
      target_z2 = source_z2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tx1") == 0)) {
      argc--;
      argv++;
      target_x1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tx2") == 0)) {
      argc--;
      argv++;
      target_x2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ty1") == 0)) {
      argc--;
      argv++;
      target_y1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ty2") == 0)) {
      argc--;
      argv++;
      target_y2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tz1") == 0)) {
      argc--;
      argv++;
      target_z1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tz2") == 0)) {
      argc--;
      argv++;
      target_z2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sx1") == 0)) {
      argc--;
      argv++;
      source_x1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sx2") == 0)) {
      argc--;
      argv++;
      source_x2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sy1") == 0)) {
      argc--;
      argv++;
      source_y1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sy2") == 0)) {
      argc--;
      argv++;
      source_y2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sz1") == 0)) {
      argc--;
      argv++;
      source_z1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sz2") == 0)) {
      argc--;
      argv++;
      source_z2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tp") == 0)) {
      argc--;
      argv++;
      target_padding = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sp") == 0)) {
      argc--;
      argv++;
      source_padding = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-invert") == 0)) {
      argc--;
      argv++;
      invert = True;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-linear") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkLinearInterpolateImageFunction;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-bspline") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkBSplineInterpolateImageFunction;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-cspline") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkCSplineInterpolateImageFunction;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-sinc") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkSincInterpolateImageFunction;
      ok = True;
    }
    if (ok == False) {
      cerr << "Can not parse argument " << argv[1] << endl;
      usage();
    }
  }

  // Create default interpolator
  if (interpolator == NULL) {
    interpolator = new irtkNearestNeighborInterpolateImageFunction;
  }

  // If there is no target image use copy of source image as target image
  if (target == NULL) {
    target = new irtkGreyImage(*source);
  }

  // Compute region of interest for target image
  if (target_x1 == -1) target_x1 = 0;
  if (target_y1 == -1) target_y1 = 0;
  if (target_z1 == -1) target_z1 = 0;
  if (target_x2 == -1) target_x2 = target->GetX();
  if (target_y2 == -1) target_y2 = target->GetY();
  if (target_z2 == -1) target_z2 = target->GetZ();

  // Compute region of interest for source image
  if (source_x1 == -1) source_x1 = 0;
  if (source_y1 == -1) source_y1 = 0;
  if (source_z1 == -1) source_z1 = 0;
  if (source_x2 == -1) source_x2 = source->GetX();
  if (source_y2 == -1) source_y2 = source->GetY();
  if (source_z2 == -1) source_z2 = source->GetZ();

  // If there is an region of interest for the target image, use it
  if ((target_x1 != 0) || (target_x2 != target->GetX()) ||
      (target_y1 != 0) || (target_y2 != target->GetY()) ||
      (target_z1 != 0) || (target_z2 != target->GetZ())) {
    *target = target->GetRegion(target_x1, target_y1, target_z1,
                                target_x2, target_y2, target_z2);
  }

  // If there is an region of interest for the source image, use it
  if ((source_x1 != 0) || (source_x2 != source->GetX()) ||
      (source_y1 != 0) || (source_y2 != source->GetY()) ||
      (source_z1 != 0) || (source_z2 != source->GetZ())) {
    *source = source->GetRegion(source_x1, source_y1, source_z1,
                                source_x2, source_y2, source_z2);
  }

  if (dof_name != NULL) {
    // Read transformation
    transformation = irtkTransformation::New(dof_name);
  } else {
    // Create identity transformation
    transformation = new irtkRigidTransformation;
  }

  // Create image transformation
  irtkImageTransformation *imagetransformation =
    new irtkImageTransformation;

  imagetransformation->SetInput (source, transformation);
  imagetransformation->SetOutput(target);
  imagetransformation->PutTargetPaddingValue(target_padding);
  imagetransformation->PutSourcePaddingValue(source_padding);
  imagetransformation->PutInterpolator(interpolator);

  // Do inverse transformation if necessary
  if (invert == True) {
    imagetransformation->InvertOn();
  } else {
    imagetransformation->InvertOff();
  }

  // Transform image
  imagetransformation->Run();

  // Write the final transformation estimate
  target->Write(output_name);
}
<commit_msg>Now preserves input image type<commit_after>/*=========================================================================

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

=========================================================================*/

#include <irtkImage.h>

#include <irtkRegionFilter.h>

#include <irtkTransformation.h>

// Default filenames
char *input_name = NULL, *output_name = NULL, *dof_name  = NULL;

void usage()
{
  cerr << "Usage: transformation [source] [output] <options>\n" << endl;
  cerr << "where <options> is one or more of the following:\n" << endl;
  cerr << "<-dofin file>      Transformation" << endl;
  cerr << "<-target file>     Target image" << endl;
  cerr << "<-Rx1 value>       Region of interest" << endl;
  cerr << "<-Ry1 value>       Region of interest" << endl;
  cerr << "<-Rz1 value>       Region of interest" << endl;
  cerr << "<-Rx2 value>       Region of interest" << endl;
  cerr << "<-Ry2 value>       Region of interest" << endl;
  cerr << "<-Rz2 value>       Region of interest" << endl;
  cerr << "<-Tx1 value>       Region of interest in target image" << endl;
  cerr << "<-Ty1 value>       Region of interest in target image" << endl;
  cerr << "<-Tz1 value>       Region of interest in target image" << endl;
  cerr << "<-Tx2 value>       Region of interest in target image" << endl;
  cerr << "<-Ty2 value>       Region of interest in target image" << endl;
  cerr << "<-Tz2 value>       Region of interest in target image" << endl;
  cerr << "<-Sx1 value>       Region of interest in source image" << endl;
  cerr << "<-Sy1 value>       Region of interest in source image" << endl;
  cerr << "<-Sz1 value>       Region of interest in source image" << endl;
  cerr << "<-Sx2 value>       Region of interest in source image" << endl;
  cerr << "<-Sy2 value>       Region of interest in source image" << endl;
  cerr << "<-Sz2 value>       Region of interest in source image" << endl;
  cerr << "<-Tp value>        Target padding value" << endl;
  cerr << "<-Sp value>        Source padding value" << endl;
  cerr << "<-invert>          Invert transformation" << endl;
  cerr << "<-linear>          Linear interpolation" << endl;
  cerr << "<-bspline>         B-spline interpolation" << endl;
  cerr << "<-cspline>         Cubic spline interpolation" << endl;
  cerr << "<-sinc>            Sinc interpolation" << endl;
  exit(1);
}

int main(int argc, char **argv)
{
  int ok, invert;
  int target_padding, source_padding;
  int target_x1, target_y1, target_z1, target_x2, target_y2, target_z2;
  int source_x1, source_y1, source_z1, source_x2, source_y2, source_z2;
  irtkTransformation *transformation = NULL;
  irtkImage *source = NULL;
  irtkImage *target = NULL;
  irtkImageFunction *interpolator = NULL;

  // Check command line
  if (argc < 3) {
    usage();
  }

  // Parse image
  input_name  = argv[1];
  argc--;
  argv++;
  output_name = argv[1];
  argc--;
  argv++;

  // Read image
  cout << "Reading image ... "; cout.flush();
  source = irtkImage::New(input_name);
  cout << "done" << endl;

  // Fix ROI
  target_x1 = -1;
  target_y1 = -1;
  target_z1 = -1;
  target_x2 = -1;
  target_y2 = -1;
  target_z2 = -1;
  source_x1 = -1;
  source_y1 = -1;
  source_z1 = -1;
  source_x2 = -1;
  source_y2 = -1;
  source_z2 = -1;

  // Other options
  invert = False;
  source_padding = 0;
  target_padding = MIN_GREY;

  while (argc > 1) {
    ok = False;
    if ((ok == False) && (strcmp(argv[1], "-dofin") == 0)) {
      argc--;
      argv++;
      dof_name = argv[1];
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-target") == 0)) {
      argc--;
      argv++;
      target = new irtkGreyImage(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rx1") == 0)) {
      argc--;
      argv++;
      target_x1 = source_x1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rx2") == 0)) {
      argc--;
      argv++;
      target_x2 = source_x2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ry1") == 0)) {
      argc--;
      argv++;
      target_y1 = source_y1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ry2") == 0)) {
      argc--;
      argv++;
      target_y2 = source_y2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rz1") == 0)) {
      argc--;
      argv++;
      target_z1 = source_z1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Rz2") == 0)) {
      argc--;
      argv++;
      target_z2 = source_z2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tx1") == 0)) {
      argc--;
      argv++;
      target_x1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tx2") == 0)) {
      argc--;
      argv++;
      target_x2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ty1") == 0)) {
      argc--;
      argv++;
      target_y1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Ty2") == 0)) {
      argc--;
      argv++;
      target_y2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tz1") == 0)) {
      argc--;
      argv++;
      target_z1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tz2") == 0)) {
      argc--;
      argv++;
      target_z2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sx1") == 0)) {
      argc--;
      argv++;
      source_x1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sx2") == 0)) {
      argc--;
      argv++;
      source_x2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sy1") == 0)) {
      argc--;
      argv++;
      source_y1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sy2") == 0)) {
      argc--;
      argv++;
      source_y2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sz1") == 0)) {
      argc--;
      argv++;
      source_z1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sz2") == 0)) {
      argc--;
      argv++;
      source_z2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Tp") == 0)) {
      argc--;
      argv++;
      target_padding = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-Sp") == 0)) {
      argc--;
      argv++;
      source_padding = atoi(argv[1]);
      argc--;
      argv++;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-invert") == 0)) {
      argc--;
      argv++;
      invert = True;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-linear") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkLinearInterpolateImageFunction;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-bspline") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkBSplineInterpolateImageFunction;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-cspline") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkCSplineInterpolateImageFunction;
      ok = True;
    }
    if ((ok == False) && (strcmp(argv[1], "-sinc") == 0)) {
      argc--;
      argv++;
      interpolator = new irtkSincInterpolateImageFunction;
      ok = True;
    }
    if (ok == False) {
      cerr << "Can not parse argument " << argv[1] << endl;
      usage();
    }
  }

  // Create default interpolator
  if (interpolator == NULL) {
    interpolator = new irtkNearestNeighborInterpolateImageFunction;
  }

  // If there is no target image use copy of source image as target image
  if (target == NULL) {
    target = irtkImage::New(source);
  }

  // Compute region of interest for target image
  if (target_x1 == -1) target_x1 = 0;
  if (target_y1 == -1) target_y1 = 0;
  if (target_z1 == -1) target_z1 = 0;
  if (target_x2 == -1) target_x2 = target->GetX();
  if (target_y2 == -1) target_y2 = target->GetY();
  if (target_z2 == -1) target_z2 = target->GetZ();

  // Compute region of interest for source image
  if (source_x1 == -1) source_x1 = 0;
  if (source_y1 == -1) source_y1 = 0;
  if (source_z1 == -1) source_z1 = 0;
  if (source_x2 == -1) source_x2 = source->GetX();
  if (source_y2 == -1) source_y2 = source->GetY();
  if (source_z2 == -1) source_z2 = source->GetZ();

  // If there is an region of interest for the target image, use it
  if ((target_x1 != 0) || (target_x2 != target->GetX()) ||
      (target_y1 != 0) || (target_y2 != target->GetY()) ||
      (target_z1 != 0) || (target_z2 != target->GetZ())) {
    irtkRegionFilter *region = new irtkRegionFilter;
    region->SetInput (target);
    region->SetOutput(target);
    region->PutRegion(target_x1, target_y1, target_z1, 0, target_x2, target_y2, target_z2, 1);
  }

  // If there is an region of interest for the source image, use it
  if ((source_x1 != 0) || (source_x2 != source->GetX()) ||
      (source_y1 != 0) || (source_y2 != source->GetY()) ||
      (source_z1 != 0) || (source_z2 != source->GetZ())) {
    irtkRegionFilter *region = new irtkRegionFilter;
    region->SetInput (source);
    region->SetOutput(source);
    region->PutRegion(source_x1, source_y1, source_z1, 0, source_x2, source_y2, source_z2, 1);
  }

  if (dof_name != NULL) {
    // Read transformation
    transformation = irtkTransformation::New(dof_name);
  } else {
    // Create identity transformation
    transformation = new irtkRigidTransformation;
  }

  // Create image transformation
  irtkImageTransformation *imagetransformation =
    new irtkImageTransformation;

  imagetransformation->SetInput (source, transformation);
  imagetransformation->SetOutput(target);
  imagetransformation->PutTargetPaddingValue(target_padding);
  imagetransformation->PutSourcePaddingValue(source_padding);
  imagetransformation->PutInterpolator(interpolator);

  // Do inverse transformation if necessary
  if (invert == True) {
    imagetransformation->InvertOn();
  } else {
    imagetransformation->InvertOff();
  }

  // Transform image
  imagetransformation->Run();

  // Write the final transformation estimate
  target->Write(output_name);
}
<|endoftext|>


<commit_before>/**
 * @file perceptron_impl.hpp
 * @author Udit Saxena
 *
 * Implementation of Perceptron class.
 */
#ifndef __MLPACK_METHODS_PERCEPTRON_PERCEPTRON_IMPL_HPP
#define __MLPACK_METHODS_PERCEPTRON_PERCEPTRON_IMPL_HPP

#include "perceptron.hpp"

namespace mlpack {
namespace perceptron {

/**
 * Constructor - constructs the perceptron. Or rather, builds the weightVectors
 * matrix, which is later used in Classification.
 * It adds a bias input vector of 1 to the input data to take care of the bias
 * weights.
 *
 * @param data Input, training data.
 * @param labels Labels of dataset.
 * @param iterations Maximum number of iterations for the perceptron learning
 *      algorithm.
 */
template<
    typename LearnPolicy,
    typename WeightInitializationPolicy,
    typename MatType
>
Perceptron<LearnPolicy, WeightInitializationPolicy, MatType>::Perceptron(
    const MatType& data,
    const arma::Row<size_t>& labels,
    int iterations)
{
  WeightInitializationPolicy WIP;
  WIP.Initialize(weightVectors, arma::max(labels) + 1, data.n_rows + 1);

  // Start training.
  classLabels = labels;

  trainData = data;
  // Insert a row of ones at the top of the training data set.
  MatType zOnes(1, data.n_cols);
  zOnes.fill(1);
  trainData.insert_rows(0, zOnes);

  int j, i = 0;
  bool converged = false;
  size_t tempLabel;
  arma::uword maxIndexRow, maxIndexCol;
  double maxVal;
  arma::mat tempLabelMat;

  LearnPolicy LP;

  while ((i < iterations) && (!converged))
  {
    // This outer loop is for each iteration, and we use the 'converged'
    // variable for noting whether or not convergence has been reached.
    i++;
    converged = true;

    // Now this inner loop is for going through the dataset in each iteration.
    for (j = 0; j < data.n_cols; j++)
    {
      // Multiply for each variable and check whether the current weight vector
      // correctly classifies this.
      tempLabelMat = weightVectors * trainData.col(j);

      maxVal = tempLabelMat.max(maxIndexRow, maxIndexCol);
      maxVal *= 2;
      // Check whether prediction is correct.
      if (maxIndexRow != classLabels(0, j))
      {
        // Due to incorrect prediction, convergence set to false.
        converged = false;
        tempLabel = labels(0, j);
        // Send maxIndexRow for knowing which weight to update, send j to know
        // the value of the vector to update it with.  Send tempLabel to know
        // the correct class.
        LP.UpdateWeights(trainData, weightVectors, j, tempLabel, maxIndexRow);
      }
    }
  }
}

/**
 * Classification function. After training, use the weightVectors matrix to
 * classify test, and put the predicted classes in predictedLabels.
 *
 * @param test testing data or data to classify.
 * @param predictedLabels vector to store the predicted classes after
 *      classifying test
 */
template <typename LearnPolicy, typename WeightInitializationPolicy, typename MatType>
void Perceptron<LearnPolicy, WeightInitializationPolicy, MatType>::Classify(
    const MatType& test,
    arma::Row<size_t>& predictedLabels)
{
  arma::mat tempLabelMat;
  arma::uword maxIndexRow, maxIndexCol;
  double maxVal;

  for (int i = 0; i < test.n_cols; i++)
  {
    tempLabelMat = weightVectors.submat(0,1,weightVectors.n_rows-1,
                                        weightVectors.n_cols-1) *
                                        test.col(i) + weightVectors.col(0);
    maxVal = tempLabelMat.max(maxIndexRow, maxIndexCol);
    maxVal *= 2;
    predictedLabels(0, i) = maxIndexRow;
  }
}

}; // namespace perceptron
}; // namespace mlpack

#endif
<commit_msg>Minor improvement. No major functionality changes<commit_after>/**
 * @file perceptron_impl.hpp
 * @author Udit Saxena
 *
 * Implementation of Perceptron class.
 */
#ifndef __MLPACK_METHODS_PERCEPTRON_PERCEPTRON_IMPL_HPP
#define __MLPACK_METHODS_PERCEPTRON_PERCEPTRON_IMPL_HPP

#include "perceptron.hpp"

namespace mlpack {
namespace perceptron {

/**
 * Constructor - constructs the perceptron. Or rather, builds the weightVectors
 * matrix, which is later used in Classification.
 * It adds a bias input vector of 1 to the input data to take care of the bias
 * weights.
 *
 * @param data Input, training data.
 * @param labels Labels of dataset.
 * @param iterations Maximum number of iterations for the perceptron learning
 *      algorithm.
 */
template<
    typename LearnPolicy,
    typename WeightInitializationPolicy,
    typename MatType
>
Perceptron<LearnPolicy, WeightInitializationPolicy, MatType>::Perceptron(
    const MatType& data,
    const arma::Row<size_t>& labels,
    int iterations)
{
  WeightInitializationPolicy WIP;
  WIP.Initialize(weightVectors, arma::max(labels) + 1, data.n_rows + 1);

  // Start training.
  classLabels = labels;

  trainData = data;
  // Insert a row of ones at the top of the training data set.
  MatType zOnes(1, data.n_cols);
  zOnes.fill(1);
  trainData.insert_rows(0, zOnes);

  int j, i = 0;
  bool converged = false;
  size_t tempLabel;
  arma::uword maxIndexRow, maxIndexCol;
  arma::mat tempLabelMat;

  LearnPolicy LP;

  while ((i < iterations) && (!converged))
  {
    // This outer loop is for each iteration, and we use the 'converged'
    // variable for noting whether or not convergence has been reached.
    i++;
    converged = true;

    // Now this inner loop is for going through the dataset in each iteration.
    for (j = 0; j < data.n_cols; j++)
    {
      // Multiply for each variable and check whether the current weight vector
      // correctly classifies this.
      tempLabelMat = weightVectors * trainData.col(j);

      tempLabelMat.max(maxIndexRow, maxIndexCol);

      // Check whether prediction is correct.
      if (maxIndexRow != classLabels(0, j))
      {
        // Due to incorrect prediction, convergence set to false.
        converged = false;
        tempLabel = labels(0, j);
        // Send maxIndexRow for knowing which weight to update, send j to know
        // the value of the vector to update it with.  Send tempLabel to know
        // the correct class.
        LP.UpdateWeights(trainData, weightVectors, j, tempLabel, maxIndexRow);
      }
    }
  }
}

/**
 * Classification function. After training, use the weightVectors matrix to
 * classify test, and put the predicted classes in predictedLabels.
 *
 * @param test testing data or data to classify.
 * @param predictedLabels vector to store the predicted classes after
 *      classifying test
 */
template <typename LearnPolicy, typename WeightInitializationPolicy, typename MatType>
void Perceptron<LearnPolicy, WeightInitializationPolicy, MatType>::Classify(
    const MatType& test,
    arma::Row<size_t>& predictedLabels)
{
  arma::mat tempLabelMat;
  arma::uword maxIndexRow, maxIndexCol;

  for (int i = 0; i < test.n_cols; i++)
  {
    tempLabelMat = weightVectors.submat(0, 1, weightVectors.n_rows - 1,
                                        weightVectors.n_cols - 1) *
                                        test.col(i) + weightVectors.col(0);
    tempLabelMat.max(maxIndexRow, maxIndexCol);
    predictedLabels(0, i) = maxIndexRow;
  }
}

}; // namespace perceptron
}; // namespace mlpack

#endif
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file TranslationControl.cpp
 *
 * This file implements a P-position-control cascaded with a
 * PID-velocity-controller.
 *
 * Inputs: vehicle states (pos, vel, q)
 *         desired setpoints (pos, vel, thrust, yaw)
 * Outputs: thrust and yaw setpoint
 */

#include "TranslationControl.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include "uORB/topics/parameter_update.h"
#include "Utility/ControlMath.hpp"
#include <lib/ecl/geo/geo.h>  //TODO: only used for wrap_pi -> move this to mathlib since
// it makes more sense

using namespace matrix;

using Data = matrix::Vector3f;

TranslationControl::TranslationControl()
{
	_Pz_h   = param_find("MPC_Z_P");
	_Pvz_h  = param_find("MPC_Z_VEL_P");
	_Ivz_h  = param_find("MPC_Z_VEL_I");
	_Dvz_h  = param_find("MPC_Z_VEL_D");
	_Pxy_h  = param_find("MPC_XY_P");
	_Pvxy_h = param_find("MPC_XY_VEL_P");
	_Ivxy_h = param_find("MPC_XY_VEL_I");
	_Dvxy_h = param_find("MPC_XY_VEL_D");
	_VelMaxXY = param_find("MPC_XY_VEL_MAX");
	_VelMaxZdown_h = param_find("MPC_Z_VEL_MAX_DN");
	_VelMaxZup_h = param_find("MPC_Z_VEL_MAX_UP");
	_ThrHover_h = param_find("MPC_THR_HOVER");
	_ThrMax_h = param_find("MPC_THR_MAX");
	_ThrMin_h = param_find("MPC_THR_MIN");
	_YawRateMax_h = param_find("MPC_MAN_Y_MAX");
	_Pyaw_h = param_find("MC_YAW_P");

	/* Set parameter the very first time. */
	_setParams();
};

void TranslationControl::updateState(const struct vehicle_local_position_s state, const Data &vel_dot,
				     const matrix::Matrix<float, 3, 3> &R)
{
	_pos = Data(&state.x);
	_vel = Data(&state.vx);
	_yaw = state.yaw;
	_vel_dot = vel_dot;
	_R = R;
}

void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint)
{
	_pos_sp = Data(&setpoint.x);
	_vel_sp = Data(&setpoint.vx);
	_acc_sp = Data(&setpoint.acc_x);
	_yaw_sp = setpoint.yaw; //integrate
	_yawspeed_sp = setpoint.yawspeed;
	_interfaceMapping();
}

void TranslationControl::generateThrustYawSetpoint(const float &dt)
{
	_updateParams();
	_positionController();
	_velocityController(dt);
	_yawController(dt);
}

void TranslationControl::_interfaceMapping()
{
	/* Respects FlightTask interface, where
	 * NAN-setpoints are of no interest and
	 * do not require control.
	 */

	/* Loop through x,y and z components of all setpoints. */
	for (int i = 0; i <= 2; i++) {

		if (PX4_ISFINITE(_pos_sp(i))) {

			/* Position control is required */

			if (!PX4_ISFINITE(_vel_sp(i))) {
				/* Velocity is not used as feedforward term. */
				_vel_sp(i) = 0.0f;
			}

		} else {

			/* Velocity controller is active without
			 * position control.
			 */
			_pos_sp(i) = _pos(i);

			if (!PX4_ISFINITE(_vel_sp(i)))  {
				/* No position/velocity controller active.
				 * Attitude will be generated from sticks directly
				 * TODO: Adjust to the new FlightTask interface
				 * that also sends thrust setpoints.
				 */
				_vel_sp(i) = _vel(i);
			}
		}
	}

	if (!PX4_ISFINITE(_yawspeed_sp)) {

		/* Targe yaw is yaw setpoint. No need for yawspped */
		_yawspeed_sp = 0.0f;

		if (!PX4_ISFINITE(_yaw_sp)) {

			/* There is no finite setpoint. The best
			 * we can do is to just re-use old setpoint */
			_yaw_sp = _yaw_sp_int;
		}

	} else if (!PX4_ISFINITE(_yaw_sp)) {
		/* Nothing is finite: Best we can do is to just
		 * reuse old setpoint.
		 */
		_yaw_sp = _yaw_sp_int;
	}
}

void TranslationControl::_positionController()
{
	/* Generate desired velocity setpoint */

	/* P-controller */
	_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;

	/* Make sure velocity setpoint is constrained in all directions (xyz). */
	float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));

	if (vel_norm_xy > _VelMaxXY) {
		_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy;
		_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy;
	}

	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
}

void TranslationControl::_velocityController(const float &dt)
{
	/* Generate desired thrust setpoint */
	/* PID
	 * u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
	 * Umin <= u_des <= Umax
	 *
	 * Saturation for vel_integral;
	 * u_des = _thr_sp; r = _vel_sp; y = _vel
	 * u_des >= Umax and r - y >= 0 => Saturation = true
	 * u_des >= Umax and r - y <= 0 => Saturation = false
	 * u_des <= Umin and r - y <= 0 => Saturation = true
	 * u_des <= Umin and r - y >= 0 => Saturation = false
	 *
	 */

	Data vel_err = _vel_sp - _vel;

	/* TODO: add offboard acceleration mode
	 * PID-controller */
	Data offset(0.0f, 0.0f, _ThrHover);
	_thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;

	/* Limit tilt with priority on z
	 * For manual controlled mode excluding pure manual and rate control, maximum tilt is 90;
	 * It is to note that pure manual and rate control will never enter _velocityController method.
	 * TODO: This needs to be revisited. */
	float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F;
	tilt_max = math::min(tilt_max, M_PI_2_F);
	_thr_sp = ControlMath::constrainTilt(_thr_sp, tilt_max);

	/*TODO: Check if it is beneficial to project thrust onto body z axis  */

	/* Calculate desired total thrust amount in body z direction. */
	/* To compensate for excess thrust during attitude tracking errors we
	 * project the desired thrust force vector F onto the real vehicle's thrust axis in NED:
	 * body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z */
//	matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2));
//	_throttle = thr_sp_tilt.dot(-R_z);
//
//	/* Re-scale thrust set-point based on throttle*/
//	if (thr_sp_tilt.length() < 0.0001f) {
//		_thr_sp = matrix::Vector3f(0.0f, 0.0f,  0.0f);
//
//	} else {
//		_thr_sp = thr_sp_tilt.normalized() * _throttle;
//	}

	/* Constrain thrust set-point and update saturation flag */
	/* To get (r-y) for horizontal direction, we look at the dot-product
	 * for vel_err and _vel_sp. The sign of the dot product indicates
	 * if (r-y) is greater or smaller than 0
	 */
	float dot_xy = matrix::Vector2f(&vel_err(0)) * matrix::Vector2f(&_vel_sp(0));
	float direction[2] = {dot_xy, -vel_err(2)}; // negative sign because of N-E-D
	bool stop_I[2] = {false, false}; // stop integration for xy and z
	ControlMath::constrainPIDu(_thr_sp, stop_I, _ThrLimit, direction);

	/* Update integrals */
	if (!stop_I[0]) {
		_thr_int(0) += vel_err(0) * Iv(0) * dt;
		_thr_int(1) += vel_err(1) * Iv(1) * dt;
	}

	if (!stop_I[1]) {
		_thr_int(2) += vel_err(2) * Iv(2) * dt;
	}

}

void TranslationControl::_yawController(const float &dt)
{
	const float yaw_offset_max = math::radians(_YawRateMax) / _Pyaw;
	const float  yaw_target = _wrap_pi(_yaw_sp + _yawspeed_sp * dt);
	const float yaw_offset = _wrap_pi(yaw_target - _yaw);

	// If the yaw offset became too big for the system to track stop
	// shifting it, only allow if it would make the offset smaller again.
	if (fabsf(yaw_offset) < yaw_offset_max || (_yawspeed_sp > 0 && yaw_offset < 0)
	    || (_yawspeed_sp < 0 && yaw_offset > 0)) {
		_yaw_sp = yaw_target;
	}

	/* Update yaw setpoint integral */
	_yaw_sp_int = _yaw_sp;

}

void TranslationControl::updateConstraints(const Controller::Constraints &constraints)
{
	_constraints = constraints;
}

void TranslationControl::_updateParams()
{
	bool updated;
	parameter_update_s param_update;
	orb_check(_parameter_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(parameter_update), _parameter_sub, &param_update);
		_setParams();
	}
}

void TranslationControl::_setParams()
{
	param_get(_Pxy_h, &Pp(0));
	param_get(_Pxy_h, &Pp(1));
	param_get(_Pz_h, &Pp(2));

	param_get(_Pvxy_h, &Pv(0));
	param_get(_Pvxy_h, &Pv(1));
	param_get(_Pvz_h, &Pv(2));

	param_get(_Ivxy_h, &Iv(0));
	param_get(_Ivxy_h, &Iv(1));
	param_get(_Ivz_h, &Iv(2));

	param_get(_Dvxy_h, &Dv(0));
	param_get(_Dvxy_h, &Dv(1));
	param_get(_Dvz_h, &Dv(2));

	param_get(_VelMaxXY_h, &_VelMaxXY);
	param_get(_VelMaxZup_h, &_VelMaxZ[0]);
	param_get(_VelMaxZdown_h, &_VelMaxZ[1]);

	param_get(_ThrHover_h, &_ThrHover);
	param_get(_ThrMax_h, &_ThrLimit[0]);
	param_get(_ThrMin_h, &_ThrLimit[1]);

	param_get(_YawRateMax_h, &_YawRateMax);
	param_get(_Pyaw_h, &_Pyaw);
}
<commit_msg>mc_pos_control: publish yaw_move_rate<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file TranslationControl.cpp
 *
 * This file implements a P-position-control cascaded with a
 * PID-velocity-controller.
 *
 * Inputs: vehicle states (pos, vel, q)
 *         desired setpoints (pos, vel, thrust, yaw)
 * Outputs: thrust and yaw setpoint
 */

#include "TranslationControl.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include "uORB/topics/parameter_update.h"
#include "Utility/ControlMath.hpp"
#include <lib/ecl/geo/geo.h>  //TODO: only used for wrap_pi -> move this to mathlib since
// it makes more sense

using namespace matrix;

using Data = matrix::Vector3f;

TranslationControl::TranslationControl()
{
	_Pz_h   = param_find("MPC_Z_P");
	_Pvz_h  = param_find("MPC_Z_VEL_P");
	_Ivz_h  = param_find("MPC_Z_VEL_I");
	_Dvz_h  = param_find("MPC_Z_VEL_D");
	_Pxy_h  = param_find("MPC_XY_P");
	_Pvxy_h = param_find("MPC_XY_VEL_P");
	_Ivxy_h = param_find("MPC_XY_VEL_I");
	_Dvxy_h = param_find("MPC_XY_VEL_D");
	_VelMaxXY = param_find("MPC_XY_VEL_MAX");
	_VelMaxZdown_h = param_find("MPC_Z_VEL_MAX_DN");
	_VelMaxZup_h = param_find("MPC_Z_VEL_MAX_UP");
	_ThrHover_h = param_find("MPC_THR_HOVER");
	_ThrMax_h = param_find("MPC_THR_MAX");
	_ThrMin_h = param_find("MPC_THR_MIN");
	_YawRateMax_h = param_find("MPC_MAN_Y_MAX");
	_Pyaw_h = param_find("MC_YAW_P");

	/* Set parameter the very first time. */
	_setParams();
};

void TranslationControl::updateState(const struct vehicle_local_position_s state, const Data &vel_dot,
				     const matrix::Matrix<float, 3, 3> &R)
{
	_pos = Data(&state.x);
	_vel = Data(&state.vx);
	_yaw = state.yaw;
	_vel_dot = vel_dot;
	_R = R;
}

void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint)
{
	_pos_sp = Data(&setpoint.x);
	_vel_sp = Data(&setpoint.vx);
	_acc_sp = Data(&setpoint.acc_x);
	_yaw_sp = setpoint.yaw; //integrate
	_yawspeed_sp = setpoint.yawspeed;
	_interfaceMapping();
}

void TranslationControl::generateThrustYawSetpoint(const float &dt)
{
	_updateParams();
	_positionController();
	_velocityController(dt);
	_yawController(dt);
}

void TranslationControl::_interfaceMapping()
{
	/* Respects FlightTask interface, where
	 * NAN-setpoints are of no interest and
	 * do not require control.
	 */

	/* Loop through x,y and z components of all setpoints. */
	for (int i = 0; i <= 2; i++) {

		if (PX4_ISFINITE(_pos_sp(i))) {

			/* Position control is required */

			if (!PX4_ISFINITE(_vel_sp(i))) {
				/* Velocity is not used as feedforward term. */
				_vel_sp(i) = 0.0f;
			}

		} else {

			/* Velocity controller is active without
			 * position control.
			 */
			_pos_sp(i) = _pos(i);

			if (!PX4_ISFINITE(_vel_sp(i)))  {
				/* No position/velocity controller active.
				 * Attitude will be generated from sticks directly
				 * TODO: Adjust to the new FlightTask interface
				 * that also sends thrust setpoints.
				 */
				_vel_sp(i) = _vel(i);
			}
		}
	}

	if (!PX4_ISFINITE(_yawspeed_sp)) {

		/* Target yaw is yaw setpoint. No need for yawspeed */
		_yawspeed_sp = 0.0f;

		if (!PX4_ISFINITE(_yaw_sp)) {

			/* There is no finite setpoint. The best
			 * we can do is to just re-use old setpoint */
			_yaw_sp = _yaw_sp_int;
		}

	} else if (!PX4_ISFINITE(_yaw_sp)) {
		/* Nothing is finite: Best we can do is to just
		 * reuse old setpoint.
		 */
		_yaw_sp = _yaw_sp_int;
	}
}

void TranslationControl::_positionController()
{
	/* Generate desired velocity setpoint */

	/* P-controller */
	_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;

	/* Make sure velocity setpoint is constrained in all directions (xyz). */
	float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));

	if (vel_norm_xy > _VelMaxXY) {
		_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy;
		_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy;
	}

	_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
}

void TranslationControl::_velocityController(const float &dt)
{
	/* Generate desired thrust setpoint */
	/* PID
	 * u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
	 * Umin <= u_des <= Umax
	 *
	 * Saturation for vel_integral;
	 * u_des = _thr_sp; r = _vel_sp; y = _vel
	 * u_des >= Umax and r - y >= 0 => Saturation = true
	 * u_des >= Umax and r - y <= 0 => Saturation = false
	 * u_des <= Umin and r - y <= 0 => Saturation = true
	 * u_des <= Umin and r - y >= 0 => Saturation = false
	 *
	 */

	Data vel_err = _vel_sp - _vel;

	/* TODO: add offboard acceleration mode
	 * PID-controller */
	Data offset(0.0f, 0.0f, _ThrHover);
	_thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;

	/* Limit tilt with priority on z
	 * For manual controlled mode excluding pure manual and rate control, maximum tilt is 90;
	 * It is to note that pure manual and rate control will never enter _velocityController method.
	 * TODO: This needs to be revisited. */
	float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F;
	tilt_max = math::min(tilt_max, M_PI_2_F);
	_thr_sp = ControlMath::constrainTilt(_thr_sp, tilt_max);

	/*TODO: Check if it is beneficial to project thrust onto body z axis  */

	/* Calculate desired total thrust amount in body z direction. */
	/* To compensate for excess thrust during attitude tracking errors we
	 * project the desired thrust force vector F onto the real vehicle's thrust axis in NED:
	 * body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z */
//	matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2));
//	_throttle = thr_sp_tilt.dot(-R_z);
//
//	/* Re-scale thrust set-point based on throttle*/
//	if (thr_sp_tilt.length() < 0.0001f) {
//		_thr_sp = matrix::Vector3f(0.0f, 0.0f,  0.0f);
//
//	} else {
//		_thr_sp = thr_sp_tilt.normalized() * _throttle;
//	}

	/* Constrain thrust set-point and update saturation flag */
	/* To get (r-y) for horizontal direction, we look at the dot-product
	 * for vel_err and _vel_sp. The sign of the dot product indicates
	 * if (r-y) is greater or smaller than 0
	 */
	float dot_xy = matrix::Vector2f(&vel_err(0)) * matrix::Vector2f(&_vel_sp(0));
	float direction[2] = {dot_xy, -vel_err(2)}; // negative sign because of N-E-D
	bool stop_I[2] = {false, false}; // stop integration for xy and z
	ControlMath::constrainPIDu(_thr_sp, stop_I, _ThrLimit, direction);

	/* Update integrals */
	if (!stop_I[0]) {
		_thr_int(0) += vel_err(0) * Iv(0) * dt;
		_thr_int(1) += vel_err(1) * Iv(1) * dt;
	}

	if (!stop_I[1]) {
		_thr_int(2) += vel_err(2) * Iv(2) * dt;
	}

}

void TranslationControl::_yawController(const float &dt)
{
	const float yaw_offset_max = math::radians(_YawRateMax) / _Pyaw;
	const float  yaw_target = _wrap_pi(_yaw_sp + _yawspeed_sp * dt);
	const float yaw_offset = _wrap_pi(yaw_target - _yaw);

	// If the yaw offset became too big for the system to track stop
	// shifting it, only allow if it would make the offset smaller again.
	if (fabsf(yaw_offset) < yaw_offset_max || (_yawspeed_sp > 0 && yaw_offset < 0)
	    || (_yawspeed_sp < 0 && yaw_offset > 0)) {
		_yaw_sp = yaw_target;
	}

	/* Update yaw setpoint integral */
	_yaw_sp_int = _yaw_sp;

}

void TranslationControl::updateConstraints(const Controller::Constraints &constraints)
{
	_constraints = constraints;
}

void TranslationControl::_updateParams()
{
	bool updated;
	parameter_update_s param_update;
	orb_check(_parameter_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(parameter_update), _parameter_sub, &param_update);
		_setParams();
	}
}

void TranslationControl::_setParams()
{
	param_get(_Pxy_h, &Pp(0));
	param_get(_Pxy_h, &Pp(1));
	param_get(_Pz_h, &Pp(2));

	param_get(_Pvxy_h, &Pv(0));
	param_get(_Pvxy_h, &Pv(1));
	param_get(_Pvz_h, &Pv(2));

	param_get(_Ivxy_h, &Iv(0));
	param_get(_Ivxy_h, &Iv(1));
	param_get(_Ivz_h, &Iv(2));

	param_get(_Dvxy_h, &Dv(0));
	param_get(_Dvxy_h, &Dv(1));
	param_get(_Dvz_h, &Dv(2));

	param_get(_VelMaxXY_h, &_VelMaxXY);
	param_get(_VelMaxZup_h, &_VelMaxZ[0]);
	param_get(_VelMaxZdown_h, &_VelMaxZ[1]);

	param_get(_ThrHover_h, &_ThrHover);
	param_get(_ThrMax_h, &_ThrLimit[0]);
	param_get(_ThrMin_h, &_ThrLimit[1]);

	param_get(_YawRateMax_h, &_YawRateMax);
	param_get(_Pyaw_h, &_Pyaw);
}
<|endoftext|>


<commit_before>#ifndef PROJECTCONFIG_HPP
#define PROJECTCONFIG_HPP

#include <cstdint>
#include <unordered_map>

#include <QtCore/QFile>
#include <QtCore/QString>
#include <QtCore/QVariant>
#include <qopenglext.h>

#include "util/Util.hpp"

namespace balls {
namespace config {
using std::uint8_t;
using std::unordered_map;

Q_DECL_DEPRECATED struct ProjectConfig {

  /// The vertex shader of this project (required)
  QString vertexShader;

  /// The fragment shader of this project (required)
  QString fragmentShader;

  /// The geometry shader of this project (optional)
  QString geometryShader;

  /// The name of each uniform and their types and values
  unordered_map<QString, QVariant> uniforms;

  /// The current value of each OpenGL state
  unordered_map<GLenum, GLenum> glState;

  /// The major version of BALLS that exported this project
  uint8_t major : 4;

  /// The minor version of BALLS that exported this project
  uint8_t minor : 4;

  /// The OpenGL major version of the computer that exported this project
  uint8_t glMajor : 4;

  /// The OpenGL minor version of the computer that exported this project
  uint8_t glMinor : 4;
};

Q_DECL_DEPRECATED void
  saveToFile(const ProjectConfig&, const QString&, const QObject*);

Q_DECL_DEPRECATED ProjectConfig loadFromFile(const QString&);
}
}

#endif // PROJECTCONFIG_HPP
<commit_msg>Move deprecation attribute in ProjectConfig<commit_after>#ifndef PROJECTCONFIG_HPP
#define PROJECTCONFIG_HPP

#include <cstdint>
#include <unordered_map>

#include <QtCore/QFile>
#include <QtCore/QString>
#include <QtCore/QVariant>
#include <qopenglext.h>

#include "util/Util.hpp"

namespace balls {
namespace config {
using std::uint8_t;
using std::unordered_map;

struct Q_DECL_DEPRECATED ProjectConfig {

  /// The vertex shader of this project (required)
  QString vertexShader;

  /// The fragment shader of this project (required)
  QString fragmentShader;

  /// The geometry shader of this project (optional)
  QString geometryShader;

  /// The name of each uniform and their types and values
  unordered_map<QString, QVariant> uniforms;

  /// The current value of each OpenGL state
  unordered_map<GLenum, GLenum> glState;

  /// The major version of BALLS that exported this project
  uint8_t major : 4;

  /// The minor version of BALLS that exported this project
  uint8_t minor : 4;

  /// The OpenGL major version of the computer that exported this project
  uint8_t glMajor : 4;

  /// The OpenGL minor version of the computer that exported this project
  uint8_t glMinor : 4;
};

Q_DECL_DEPRECATED void
  saveToFile(const ProjectConfig&, const QString&, const QObject*);

Q_DECL_DEPRECATED ProjectConfig loadFromFile(const QString&);
}
}

#endif // PROJECTCONFIG_HPP
<|endoftext|>


<commit_before>/*
 * FogLAMP storage service.
 *
 * Copyright (c) 2017 OSisoft, LLC
 *
 * Released under the Apache 2.0 Licence
 *
 * Author: Mark Riddoch
 */
#include <management_client.h>
#include <rapidjson/document.h>
#include <service_record.h>
#include <string>
#include <sstream>
#include <iostream>

using namespace std;
using namespace rapidjson;
using HttpClient = SimpleWeb::Client<SimpleWeb::HTTP>;

/**
 * Management Client constructor
 */
ManagementClient::ManagementClient(const string& hostname, const unsigned short port) : m_uuid(0)
{
ostringstream urlbase;

	m_logger = Logger::getLogger();
	urlbase << hostname << ":" << port;
	m_client = new HttpClient(urlbase.str());
}

/**
 * Destructor for management client
 */
ManagementClient::~ManagementClient()
{
	if (m_uuid)
	{
		delete m_uuid;
		m_uuid = 0;
	}
	delete m_client;
}

/**
 * Register this service
 */
bool ManagementClient::registerService(const ServiceRecord& service)
{
string payload;

	try {
		service.asJSON(payload);
		auto res = m_client->request("POST", "/foglamp/service", payload);
		Document doc;
		doc.Parse(res->content.string().c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of registration: %s\n",
				res->content.string().c_str());
			return false;
		}
		if (doc.HasMember("id"))
		{
			m_uuid = new string(doc["id"].GetString());
			m_logger->info("Registered service %s.\n", m_uuid->c_str());
			return true;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to register service: %s.",
				doc["message"].GetString());
		}
		else
		{
			m_logger->error("Unexpected result from service registration %s", res->content.string().c_str());
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Register service failed %s.", e.what());
		return false;
	}
	return false;
}

/**
 * Unregister this service
 */
bool ManagementClient::unregisterService()
{

	if (!m_uuid)
	{
		return false;	// Not registered
	}
	try {
		string url = "/foglamp/service/";
		url += *m_uuid;
		auto res = m_client->request("DELETE", url.c_str());
		Document doc;
		doc.Parse(res->content.string().c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of unregistration: %s\n",
				res->content.string().c_str());
			return false;
		}
		if (doc.HasMember("id"))
		{
			m_uuid = new string(doc["id"].GetString());
			m_logger->info("Unregistered service %s.\n", m_uuid->c_str());
			return true;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to unregister service: %s.",
				doc["message"].GetString());
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Unregister service failed %s.", e.what());
		return false;
	}
	return false;
}

/**
 * Get the specified service
 */
bool ManagementClient::getService(ServiceRecord& service)
{
string payload;

	try {
		string url = "/foglamp/service";
		if (!service.getName().empty())
		{
			url += "?name=" + service.getName();
		}
		else if (!service.getType().empty())
		{
			url += "?type=" + service.getType();
		}
		auto res = m_client->request("GET", url.c_str());
		Document doc;
		doc.Parse(res->content.string().c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of fetching service record: %s\n",
				res->content.string().c_str());
			return false;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to register service: %s.",
				doc["message"].GetString());
			return false;
		}
		else
		{
			Value& serviceRecord = doc["services"][0];
			service.setAddress(serviceRecord["address"].GetString());
			service.setPort(serviceRecord["service_port"].GetInt());
			service.setProtocol(serviceRecord["protocol"].GetString());
			service.setManagementPort(serviceRecord["management_port"].GetInt());
			return true;
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get service failed %s.", e.what());
		return false;
	}
	return false;
}

/**
 * Register interest in a configuration category
 */
bool ManagementClient::registerCategory(const string& category)
{
ostringstream convert;

	if (m_uuid == 0)
	{
		// Not registered with core
		m_logger->error("Storage service is not registered with the core - not registering configuration interest");
		return true;
	}
	try {
		convert << "{ \"category\" : \"" << category << "\", ";
		convert << "\"service\" : \"" << *m_uuid << "\" }";
		auto res = m_client->request("POST", "/foglamp/interest", convert.str());
		Document doc;
		string content = res->content.string();
		doc.Parse(content.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of category registration: %s\n",
				res->content.string().c_str());
			return false;
		}
		if (doc.HasMember("id"))
		{
			const char *reg_id = doc["id"].GetString();
			m_categories[category] = string(reg_id);
			m_logger->info("Registered configuration category %s, registration id %s.",
					category.c_str(), reg_id);
			return true;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to register configuration category: %s.",
				doc["message"].GetString());
		}
		else
		{
			m_logger->error("Failed to register configuration category: %s.",
					content.c_str());
		}
	} catch (const SimpleWeb::system_error &e) {
                m_logger->error("Register configuration category failed %s.", e.what());
                return false;
        }
        return false;
}

/**
 * Unegister interest in a configuration category
 */
bool ManagementClient::unregisterCategory(const string& category)
{
ostringstream convert;

        try {
		string url = "/foglamp/interest/";
		url += m_categories[category];
                auto res = m_client->request("DELETE", url.c_str());
        } catch (const SimpleWeb::system_error &e) {
                m_logger->error("Unregister configuration category failed %s.", e.what());
                return false;
        }
        return false;
}

/**
 * Get the set of all categories from the core micro service.
 */
ConfigCategories ManagementClient::getCategories()
{
	try {
		string url = "/foglamp/service/category";
		auto res = m_client->request("GET", url.c_str());
		Document doc;
		doc.Parse(res->content.string().c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of fetching configuration categories: %s\n",
				res->content.string().c_str());
			throw new exception();
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to fetch configuration categories: %s.",
				doc["message"].GetString());
			throw new exception();
		}
		else
		{
			return ConfigCategories(res->content.string());
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get config categories failed %s.", e.what());
		throw;
	}
}

/**
 * Return the content of the named category by calling the
 * management API of the FogLAMP core.
 *
 * @param categoryName	The name of the categpry to return
 * @return ConfigCategory	The configuration category
 * @throw	exception	If the category does not exist or theresult can not be parsed
 */
ConfigCategory ManagementClient::getCategory(const string& categoryName)
{
	try {
		string url = "/foglamp/service/category/" + categoryName;
		auto res = m_client->request("GET", url.c_str());
		Document doc;
		doc.Parse(res->content.string().c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of fetching configuration category: %s\n",
				res->content.string().c_str());
			throw new exception();
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to fetch configuration category: %s.",
				doc["message"].GetString());
			throw new exception();
		}
		else
		{
			return ConfigCategory(categoryName, res->content.string());
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get config category failed %s.", e.what());
		throw;
	}
}

/**
 * Add/Update a category in the configuration manager
 */
bool ManagementClient::addCategory(const ConfigCategory& category)
{
	try {
		string url = "/foglamp/service/category";
		auto res = m_client->request("POST", url.c_str(), category.toJSON());
		Document doc;
		doc.Parse(res->content.string().c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of adding a category: %s\n",
				res->content.string().c_str());
			return false;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to add configuration category: %s.",
				doc["message"].GetString());
			return false;
		}
		else
		{
			return true;
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get config categories failed %s.", e.what());
	}
	return false;
}
<commit_msg>Fix issue with reprocessing https responses<commit_after>/*
 * FogLAMP storage service.
 *
 * Copyright (c) 2017 OSisoft, LLC
 *
 * Released under the Apache 2.0 Licence
 *
 * Author: Mark Riddoch
 */
#include <management_client.h>
#include <rapidjson/document.h>
#include <service_record.h>
#include <string>
#include <sstream>
#include <iostream>

using namespace std;
using namespace rapidjson;
using HttpClient = SimpleWeb::Client<SimpleWeb::HTTP>;

/**
 * Management Client constructor
 */
ManagementClient::ManagementClient(const string& hostname, const unsigned short port) : m_uuid(0)
{
ostringstream urlbase;

	m_logger = Logger::getLogger();
	urlbase << hostname << ":" << port;
	m_client = new HttpClient(urlbase.str());
}

/**
 * Destructor for management client
 */
ManagementClient::~ManagementClient()
{
	if (m_uuid)
	{
		delete m_uuid;
		m_uuid = 0;
	}
	delete m_client;
}

/**
 * Register this service
 */
bool ManagementClient::registerService(const ServiceRecord& service)
{
string payload;

	try {
		service.asJSON(payload);
		auto res = m_client->request("POST", "/foglamp/service", payload);
		Document doc;
		string response = res->content.string();
		doc.Parse(response.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of registration: %s\n",
				response.c_str());
			return false;
		}
		if (doc.HasMember("id"))
		{
			m_uuid = new string(doc["id"].GetString());
			m_logger->info("Registered service %s.\n", m_uuid->c_str());
			return true;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to register service: %s.",
				doc["message"].GetString());
		}
		else
		{
			m_logger->error("Unexpected result from service registration %s", response.c_str());
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Register service failed %s.", e.what());
		return false;
	}
	return false;
}

/**
 * Unregister this service
 */
bool ManagementClient::unregisterService()
{

	if (!m_uuid)
	{
		return false;	// Not registered
	}
	try {
		string url = "/foglamp/service/";
		url += *m_uuid;
		auto res = m_client->request("DELETE", url.c_str());
		Document doc;
		string response = res->content.string();
		doc.Parse(response.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of unregistration: %s\n",
				response.c_str());
			return false;
		}
		if (doc.HasMember("id"))
		{
			m_uuid = new string(doc["id"].GetString());
			m_logger->info("Unregistered service %s.\n", m_uuid->c_str());
			return true;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to unregister service: %s.",
				doc["message"].GetString());
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Unregister service failed %s.", e.what());
		return false;
	}
	return false;
}

/**
 * Get the specified service
 */
bool ManagementClient::getService(ServiceRecord& service)
{
string payload;

	try {
		string url = "/foglamp/service";
		if (!service.getName().empty())
		{
			url += "?name=" + service.getName();
		}
		else if (!service.getType().empty())
		{
			url += "?type=" + service.getType();
		}
		auto res = m_client->request("GET", url.c_str());
		Document doc;
		string response = res->content.string();
		doc.Parse(response.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of fetching service record: %s\n",
				response.c_str());
			return false;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to register service: %s.",
				doc["message"].GetString());
			return false;
		}
		else
		{
			Value& serviceRecord = doc["services"][0];
			service.setAddress(serviceRecord["address"].GetString());
			service.setPort(serviceRecord["service_port"].GetInt());
			service.setProtocol(serviceRecord["protocol"].GetString());
			service.setManagementPort(serviceRecord["management_port"].GetInt());
			return true;
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get service failed %s.", e.what());
		return false;
	}
	return false;
}

/**
 * Register interest in a configuration category
 */
bool ManagementClient::registerCategory(const string& category)
{
ostringstream convert;

	if (m_uuid == 0)
	{
		// Not registered with core
		m_logger->error("Storage service is not registered with the core - not registering configuration interest");
		return true;
	}
	try {
		convert << "{ \"category\" : \"" << category << "\", ";
		convert << "\"service\" : \"" << *m_uuid << "\" }";
		auto res = m_client->request("POST", "/foglamp/interest", convert.str());
		Document doc;
		string content = res->content.string();
		doc.Parse(content.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of category registration: %s\n",
				content.c_str());
			return false;
		}
		if (doc.HasMember("id"))
		{
			const char *reg_id = doc["id"].GetString();
			m_categories[category] = string(reg_id);
			m_logger->info("Registered configuration category %s, registration id %s.",
					category.c_str(), reg_id);
			return true;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to register configuration category: %s.",
				doc["message"].GetString());
		}
		else
		{
			m_logger->error("Failed to register configuration category: %s.",
					content.c_str());
		}
	} catch (const SimpleWeb::system_error &e) {
                m_logger->error("Register configuration category failed %s.", e.what());
                return false;
        }
        return false;
}

/**
 * Unegister interest in a configuration category
 */
bool ManagementClient::unregisterCategory(const string& category)
{
ostringstream convert;

        try {
		string url = "/foglamp/interest/";
		url += m_categories[category];
                auto res = m_client->request("DELETE", url.c_str());
        } catch (const SimpleWeb::system_error &e) {
                m_logger->error("Unregister configuration category failed %s.", e.what());
                return false;
        }
        return false;
}

/**
 * Get the set of all categories from the core micro service.
 */
ConfigCategories ManagementClient::getCategories()
{
	try {
		string url = "/foglamp/service/category";
		auto res = m_client->request("GET", url.c_str());
		Document doc;
		string response = res->content.string();
		doc.Parse(response.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of fetching configuration categories: %s\n",
				response.c_str());
			throw new exception();
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to fetch configuration categories: %s.",
				doc["message"].GetString());
			throw new exception();
		}
		else
		{
			return ConfigCategories(response);
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get config categories failed %s.", e.what());
		throw;
	}
}

/**
 * Return the content of the named category by calling the
 * management API of the FogLAMP core.
 *
 * @param categoryName	The name of the categpry to return
 * @return ConfigCategory	The configuration category
 * @throw	exception	If the category does not exist or theresult can not be parsed
 */
ConfigCategory ManagementClient::getCategory(const string& categoryName)
{
	try {
		string url = "/foglamp/service/category/" + categoryName;
		auto res = m_client->request("GET", url.c_str());
		Document doc;
		string response = res->content.string();
		doc.Parse(response.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of fetching configuration category: %s\n",
				res->content.string().c_str());
			throw new exception();
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to fetch configuration category: %s.",
				doc["message"].GetString());
			throw new exception();
		}
		else
		{
			return ConfigCategory(categoryName, response);
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get config category failed %s.", e.what());
		throw;
	}
}

/**
 * Add/Update a category in the configuration manager
 */
bool ManagementClient::addCategory(const ConfigCategory& category)
{
	try {
		string url = "/foglamp/service/category";
		auto res = m_client->request("POST", url.c_str(), category.toJSON());
		Document doc;
		string response = res->content.string();
		doc.Parse(response.c_str());
		if (doc.HasParseError())
		{
			m_logger->error("Failed to parse result of adding a category: %s\n",
				response.c_str());
			return false;
		}
		else if (doc.HasMember("message"))
		{
			m_logger->error("Failed to add configuration category: %s.",
				doc["message"].GetString());
			return false;
		}
		else
		{
			return true;
		}
	} catch (const SimpleWeb::system_error &e) {
		m_logger->error("Get config categories failed %s.", e.what());
	}
	return false;
}
<|endoftext|>


<commit_before>/* Copyright 2017 QReal Research Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include "include/iotikRuCGeneratorLibrary/iotikRuCGeneratorPluginBase.h"
#include "include/iotikRuCGeneratorLibrary/iotikRuCGeneratorDefs.h"

#include <QtCore/QDir>
#include <QtCore/QFile>
#include <QtCore/QProcess>

#include <qrkernel/settingsManager.h>
#include <iotikKit/communication/usbRobotCommunicationThread.h>

#include <iotikGeneratorBase/iotikGeneratorPluginBase.h>
//#include <iotikGeneratorBase/robotModel/generatorModelExtensionInterface.h>
#include <iotikKit/robotModel/iotikRobotModelBase.h>

#include "iotikRuCMasterGenerator.h"

using namespace iotik::communication;
using namespace iotik::ruc;
using namespace kitBase::robotModel;
using namespace qReal;

//const Id robotDiagramType = Id("RobotsMetamodel", "RobotsDiagram", "RobotsDiagramNode");
//const Id subprogramDiagramType = Id("RobotsMetamodel", "RobotsDiagram", "SubprogramDiagram");

IotikRuCGeneratorPluginBase::IotikRuCGeneratorPluginBase(
		iotik::robotModel::IotikRobotModelBase * const robotModel
		, kitBase::blocksBase::BlocksFactoryInterface * const blocksFactory
		, const QStringList &pathsToTemplates)
	: IotikGeneratorPluginBase(robotModel, blocksFactory)
	, mGenerateCodeAction(new QAction(nullptr))
	, mUsbUploadAction(new QAction(nullptr))
	, mWifiUploadAction(new QAction(nullptr))
	, mSeparator(new QAction(nullptr))
	, mActivateAction(new QAction(nullptr))
	, mUsbCommunicator(new UsbRobotCommunicationThread())
	, mRobotModel(*robotModel)
	, mPathsToTemplates(pathsToTemplates)
{
}

IotikRuCGeneratorPluginBase::~IotikRuCGeneratorPluginBase()
{
}

QList<qReal::ActionInfo> IotikRuCGeneratorPluginBase::customActions()
{
	mGenerateCodeAction->setObjectName("generateRuCCode");
	mGenerateCodeAction->setText(tr("Generate RuC code"));
	mGenerateCodeAction->setIcon(QIcon(":/iotik/ruc/images/generateRuCCode.svg"));
	qReal::ActionInfo generateCodeActionInfo(mGenerateCodeAction, "generators", "tools");
	connect(mGenerateCodeAction, SIGNAL(triggered()), this, SLOT(generateCode()), Qt::UniqueConnection);

	mUsbUploadAction->setObjectName("usbUpload");
	mUsbUploadAction->setText(tr("Upload program by USB"));
	mUsbUploadAction->setIcon(QIcon(":/iotik/ruc/images/usbUpload.svg"));
	qReal::ActionInfo usbUploadActionInfo(mUsbUploadAction, "generators", "tools");
	connect(mUsbUploadAction, SIGNAL(triggered()), this, SLOT(usbUpload()), Qt::UniqueConnection);

	mWifiUploadAction->setObjectName("wifiUpload");
	mWifiUploadAction->setText(tr("Upload program by Wi-Fi"));
	mWifiUploadAction->setIcon(QIcon(":/iotik/ruc/images/wifiUpload.svg"));
	qReal::ActionInfo wifiUploadActionInfo(mWifiUploadAction, "generators", "tools");
	connect(mWifiUploadAction, SIGNAL(triggered()), this, SLOT(wifiUpload()), Qt::UniqueConnection);

	mSeparator->setSeparator(true);
	qReal::ActionInfo separatorActionInfo(mSeparator, "generators", "tools");

	mActivateAction->setObjectName("activate");
	mActivateAction->setText(tr("Аctivate IoTik v1.0"));
	mActivateAction->setIcon(QIcon(":/iotik/ruc/images/activate.svg"));
	qReal::ActionInfo activateActionInfo(mActivateAction, "generators", "tools");
	connect(mActivateAction, SIGNAL(triggered()), this, SLOT(activate()), Qt::UniqueConnection);

	return {generateCodeActionInfo, usbUploadActionInfo, wifiUploadActionInfo, separatorActionInfo, activateActionInfo};
}

QList<qReal::HotKeyActionInfo> IotikRuCGeneratorPluginBase::hotKeyActions()
{
	mGenerateCodeAction->setShortcut(QKeySequence(Qt::CTRL + Qt::Key_G));
	mUsbUploadAction->setShortcut(QKeySequence(Qt::CTRL + Qt::Key_U));
	mWifiUploadAction->setShortcut(QKeySequence(Qt::CTRL + Qt::Key_W));

	qReal::HotKeyActionInfo generateCodeInfo("Generator.GenerateRuC", tr("Generate RuC Code"), mGenerateCodeAction);
	qReal::HotKeyActionInfo usbUploadInfo("Generator.UsbUploadRuC", tr("Upload RuC Program by USB"), mUsbUploadAction);
	qReal::HotKeyActionInfo wifiUploadInfo("Generator.WifiUploadRuC", tr("Upload RuC Program by Wi-Fi"), mWifiUploadAction);

	return {generateCodeInfo, usbUploadInfo, wifiUploadInfo};
}

QIcon IotikRuCGeneratorPluginBase::iconForFastSelector(const kitBase::robotModel::RobotModelInterface &robotModel) const
{
	Q_UNUSED(robotModel)

	return QIcon(":/iotik/ruc/images/switch-to-iotik-ruc.svg");
}

generatorBase::MasterGeneratorBase *IotikRuCGeneratorPluginBase::masterGenerator()
{
	return new IotikRuCMasterGenerator(*mRepo
			, *mMainWindowInterface->errorReporter()
			, *mParserErrorReporter
			, *mRobotModelManager
			, *mTextLanguage
			, mMainWindowInterface->activeDiagram()
			, mPathsToTemplates);
}

QString IotikRuCGeneratorPluginBase::defaultFilePath(const QString &projectName) const
{
	return QString("ruc/%1/%1.c").arg(projectName);
}

qReal::text::LanguageInfo IotikRuCGeneratorPluginBase::language() const
{
	return qReal::text::Languages::c();
}

QString IotikRuCGeneratorPluginBase::generatorName() const
{
	return "iotikRuC";
}

void IotikRuCGeneratorPluginBase::activate()
{
	mMainWindowInterface->errorReporter()->addError(tr("Activator is not released yet"));
}

void IotikRuCGeneratorPluginBase::wifiUpload()
{
	mMainWindowInterface->errorReporter()->addError(tr("Wi-Fi is not released yet"));
}

void IotikRuCGeneratorPluginBase::usbUpload()
{
	const QFileInfo fileInfo = generateCodeForProcessing();
	const QString rootPath = QDir::current().absolutePath();

	if (!compileCode(fileInfo)) {
		return;
	}
	configureSensors();

	mUsbCommunicator->connect();
	mUsbCommunicator->sendCommand("cd flash\n");
	mUsbCommunicator->sendFile("sensors");
	mUsbCommunicator->sendFile("export");
	mUsbCommunicator->sendCommand("ruc export sensors\n");
	mUsbCommunicator->disconnect();

	QFile::remove(rootPath + "/sensors");
	QFile::remove(rootPath + "/export");
}

bool IotikRuCGeneratorPluginBase::compileCode(const QFileInfo fileInfo)
{
	const QString rootPath = QDir::current().absolutePath();
	const QString filePath = fileInfo.absoluteFilePath();

	if (QFile::exists(RUC_COMPILER)) {
		QProcess::execute(RUC_COMPILER, {filePath});
	} else {
		QProcess::execute(RUC_DEBUG_COMPILER, {filePath});
	}

	QFile::remove(rootPath + "/tree.txt");
	QFile::remove(rootPath + "/codes.txt");

	if (QFile::exists(rootPath + "/export.txt")) {
		QFile::rename(rootPath + "/export.txt", rootPath + "/export");
	} else {
		mMainWindowInterface->errorReporter()->addError(tr("Code compiling failed, aborting"));
		return false;
	}

	return true;
}

void IotikRuCGeneratorPluginBase::configureSensors()
{
	const QString rootPath = QDir::current().absolutePath();

	QFile sensors(rootPath + "/sensors");
	sensors.open(QIODevice::WriteOnly);
	QTextStream out(&sensors);

	out << "servos 5 12 4 13\n";
	out << "digital 1\n";
	out << "0 1 14 15\n";
	out << "analog 1\n";
	out << "0 1\n";

	sensors.close();
}
<commit_msg>Fix long line<commit_after>/* Copyright 2017 QReal Research Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include "include/iotikRuCGeneratorLibrary/iotikRuCGeneratorPluginBase.h"
#include "include/iotikRuCGeneratorLibrary/iotikRuCGeneratorDefs.h"

#include <QtCore/QDir>
#include <QtCore/QFile>
#include <QtCore/QProcess>

#include <qrkernel/settingsManager.h>
#include <iotikKit/communication/usbRobotCommunicationThread.h>

#include <iotikGeneratorBase/iotikGeneratorPluginBase.h>
//#include <iotikGeneratorBase/robotModel/generatorModelExtensionInterface.h>
#include <iotikKit/robotModel/iotikRobotModelBase.h>

#include "iotikRuCMasterGenerator.h"

using namespace iotik::communication;
using namespace iotik::ruc;
using namespace kitBase::robotModel;
using namespace qReal;

//const Id robotDiagramType = Id("RobotsMetamodel", "RobotsDiagram", "RobotsDiagramNode");
//const Id subprogramDiagramType = Id("RobotsMetamodel", "RobotsDiagram", "SubprogramDiagram");

IotikRuCGeneratorPluginBase::IotikRuCGeneratorPluginBase(
		iotik::robotModel::IotikRobotModelBase * const robotModel
		, kitBase::blocksBase::BlocksFactoryInterface * const blocksFactory
		, const QStringList &pathsToTemplates)
	: IotikGeneratorPluginBase(robotModel, blocksFactory)
	, mGenerateCodeAction(new QAction(nullptr))
	, mUsbUploadAction(new QAction(nullptr))
	, mWifiUploadAction(new QAction(nullptr))
	, mSeparator(new QAction(nullptr))
	, mActivateAction(new QAction(nullptr))
	, mUsbCommunicator(new UsbRobotCommunicationThread())
	, mRobotModel(*robotModel)
	, mPathsToTemplates(pathsToTemplates)
{
}

IotikRuCGeneratorPluginBase::~IotikRuCGeneratorPluginBase()
{
}

QList<qReal::ActionInfo> IotikRuCGeneratorPluginBase::customActions()
{
	mGenerateCodeAction->setObjectName("generateRuCCode");
	mGenerateCodeAction->setText(tr("Generate RuC code"));
	mGenerateCodeAction->setIcon(QIcon(":/iotik/ruc/images/generateRuCCode.svg"));
	qReal::ActionInfo generateCodeActionInfo(mGenerateCodeAction, "generators", "tools");
	connect(mGenerateCodeAction, SIGNAL(triggered()), this, SLOT(generateCode()), Qt::UniqueConnection);

	mUsbUploadAction->setObjectName("usbUpload");
	mUsbUploadAction->setText(tr("Upload program by USB"));
	mUsbUploadAction->setIcon(QIcon(":/iotik/ruc/images/usbUpload.svg"));
	qReal::ActionInfo usbUploadActionInfo(mUsbUploadAction, "generators", "tools");
	connect(mUsbUploadAction, SIGNAL(triggered()), this, SLOT(usbUpload()), Qt::UniqueConnection);

	mWifiUploadAction->setObjectName("wifiUpload");
	mWifiUploadAction->setText(tr("Upload program by Wi-Fi"));
	mWifiUploadAction->setIcon(QIcon(":/iotik/ruc/images/wifiUpload.svg"));
	qReal::ActionInfo wifiUploadActionInfo(mWifiUploadAction, "generators", "tools");
	connect(mWifiUploadAction, SIGNAL(triggered()), this, SLOT(wifiUpload()), Qt::UniqueConnection);

	mSeparator->setSeparator(true);
	qReal::ActionInfo separatorActionInfo(mSeparator, "generators", "tools");

	mActivateAction->setObjectName("activate");
	mActivateAction->setText(tr("Аctivate IoTik v1.0"));
	mActivateAction->setIcon(QIcon(":/iotik/ruc/images/activate.svg"));
	qReal::ActionInfo activateActionInfo(mActivateAction, "generators", "tools");
	connect(mActivateAction, SIGNAL(triggered()), this, SLOT(activate()), Qt::UniqueConnection);

	return {generateCodeActionInfo, usbUploadActionInfo, wifiUploadActionInfo,
				separatorActionInfo, activateActionInfo};
}

QList<qReal::HotKeyActionInfo> IotikRuCGeneratorPluginBase::hotKeyActions()
{
	mGenerateCodeAction->setShortcut(QKeySequence(Qt::CTRL + Qt::Key_G));
	mUsbUploadAction->setShortcut(QKeySequence(Qt::CTRL + Qt::Key_U));
	mWifiUploadAction->setShortcut(QKeySequence(Qt::CTRL + Qt::Key_W));

	qReal::HotKeyActionInfo generateCodeInfo("Generator.GenerateRuC", tr("Generate RuC Code"),
											 mGenerateCodeAction);

	qReal::HotKeyActionInfo usbUploadInfo("Generator.UsbUploadRuC", tr("Upload RuC Program by USB"),
										  mUsbUploadAction);

	qReal::HotKeyActionInfo wifiUploadInfo("Generator.WifiUploadRuC", tr("Upload RuC Program by Wi-Fi"),
										   mWifiUploadAction);

	return {generateCodeInfo, usbUploadInfo, wifiUploadInfo};
}

QIcon IotikRuCGeneratorPluginBase::iconForFastSelector(const kitBase::robotModel::RobotModelInterface &robotModel) const
{
	Q_UNUSED(robotModel)

	return QIcon(":/iotik/ruc/images/switch-to-iotik-ruc.svg");
}

generatorBase::MasterGeneratorBase *IotikRuCGeneratorPluginBase::masterGenerator()
{
	return new IotikRuCMasterGenerator(*mRepo
			, *mMainWindowInterface->errorReporter()
			, *mParserErrorReporter
			, *mRobotModelManager
			, *mTextLanguage
			, mMainWindowInterface->activeDiagram()
			, mPathsToTemplates);
}

QString IotikRuCGeneratorPluginBase::defaultFilePath(const QString &projectName) const
{
	return QString("ruc/%1/%1.c").arg(projectName);
}

qReal::text::LanguageInfo IotikRuCGeneratorPluginBase::language() const
{
	return qReal::text::Languages::c();
}

QString IotikRuCGeneratorPluginBase::generatorName() const
{
	return "iotikRuC";
}

void IotikRuCGeneratorPluginBase::activate()
{
	mMainWindowInterface->errorReporter()->addError(tr("Activator is not released yet"));
}

void IotikRuCGeneratorPluginBase::wifiUpload()
{
	mMainWindowInterface->errorReporter()->addError(tr("Wi-Fi is not released yet"));
}

void IotikRuCGeneratorPluginBase::usbUpload()
{
	const QFileInfo fileInfo = generateCodeForProcessing();
	const QString rootPath = QDir::current().absolutePath();

	if (!compileCode(fileInfo)) {
		return;
	}
	configureSensors();

	mUsbCommunicator->connect();
	mUsbCommunicator->sendCommand("cd flash\n");
	mUsbCommunicator->sendFile("sensors");
	mUsbCommunicator->sendFile("export");
	mUsbCommunicator->sendCommand("ruc export sensors\n");
	mUsbCommunicator->disconnect();

	QFile::remove(rootPath + "/sensors");
	QFile::remove(rootPath + "/export");
}

bool IotikRuCGeneratorPluginBase::compileCode(const QFileInfo fileInfo)
{
	const QString rootPath = QDir::current().absolutePath();
	const QString filePath = fileInfo.absoluteFilePath();

	if (QFile::exists(RUC_COMPILER)) {
		QProcess::execute(RUC_COMPILER, {filePath});
	} else {
		QProcess::execute(RUC_DEBUG_COMPILER, {filePath});
	}

	QFile::remove(rootPath + "/tree.txt");
	QFile::remove(rootPath + "/codes.txt");

	if (QFile::exists(rootPath + "/export.txt")) {
		QFile::rename(rootPath + "/export.txt", rootPath + "/export");
	} else {
		mMainWindowInterface->errorReporter()->addError(tr("Code compiling failed, aborting"));
		return false;
	}

	return true;
}

void IotikRuCGeneratorPluginBase::configureSensors()
{
	const QString rootPath = QDir::current().absolutePath();

	QFile sensors(rootPath + "/sensors");
	sensors.open(QIODevice::WriteOnly);
	QTextStream out(&sensors);

	out << "servos 5 12 4 13\n";
	out << "digital 1\n";
	out << "0 1 14 15\n";
	out << "analog 1\n";
	out << "0 1\n";

	sensors.close();
}
<|endoftext|>