PlugBoard::PlugBoard( char *pbFileName )
{
/* Error and format check file */
Errors checkPlugBoard;
checkPlugBoard.noRepContFile( pbFileName, 'p' );
/* Open plugboard file. Loop through the file, counting
* the total number of values in the file */
ifstream pbfile;
pbfile.open( pbFileName );
int contact;
mapLength = 0; // set @mapLength to zero
if( pbfile.fail() ){ // Throw error if cannot open
throw FileOpenErrExptn( ERROR_OPENING_CONFIGURATION_FILE, pbFileName );
}
while( !pbfile.eof() ){ // Loop through file counting values
if( !pbfile.eof() ){
pbfile >> contact;
mapLength++;
}
}
Reflector::Reflector( char *rfFileName )
{
/* Error and format check file */
Errors checkReflector;
checkReflector.noRepContFile( rfFileName, 'f' );
/* Open given reflector file. */
ifstream reflectorFile;
reflectorFile.open( rfFileName );
if( reflectorFile.fail() ){ // Throw error if cannot open
throw FileOpenErrExptn( ERROR_OPENING_CONFIGURATION_FILE, rfFileName );
}
/* Initialize elements of reflectorMap, by convering the
* int values from given file into uppercase characters
* and assign these characters to elements in @reflectorMap.
* Each column contains a 'wired' pair of characters. */
int contactOne; // first value in 'wired' pair from file
int contactTwo; // second value in 'wired' pair from file
int pair = 0;
while( !reflectorFile.eof() ){
reflectorFile >> contactOne;
reflectorFile >> contactTwo;
if( !reflectorFile.eof() ){
reflectorMap[pair][0] = (contactOne+65); // +65 for u.case convert
reflectorMap[pair][1] = (contactTwo+65);
pair++;
}
}
}
/* ************************************************************************* */
Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back((*Ai) * x);
}
return e;
}
/* ************************************************************************* */
Errors Errors::operator-(const Errors& b) const {
#ifndef NDEBUG
size_t m = size();
if (b.size()!=m)
throw(std::invalid_argument("Errors::operator-: incompatible sizes"));
#endif
Errors result;
Errors::const_iterator it = b.begin();
BOOST_FOREACH(const Vector& ai, *this)
result.push_back(ai - *(it++));
return result;
}
/* ************************************************************************* */
double dot(const Errors& a, const Errors& b) {
#ifndef NDEBUG
size_t m = a.size();
if (b.size()!=m)
throw(std::invalid_argument("Errors::dot: incompatible sizes"));
#endif
double result = 0.0;
Errors::const_iterator it = b.begin();
BOOST_FOREACH(const Vector& ai, a)
result += gtsam::dot(ai, *(it++));
return result;
}
bool SickS300::getData(LaserScannerData& data, Errors& error) {
// Bouml preserved body begin 000211E7
if (!this->open(error)) {
return false;
}
try {
if (newDataFlagOne == true) {
{
boost::mutex::scoped_lock dataMutex1(mutexData1);
data.setMeasurements(distanceBufferOne, angleBufferOne, si::meter, radian); //TODO dictance in centimeter
}
newDataFlagOne = false;
} else if (newDataFlagTwo == true) {
{
boost::mutex::scoped_lock dataMutex2(mutexData2);
data.setMeasurements(distanceBufferTwo, angleBufferTwo, meter, radian); //TODO dictance in centimeter
}
newDataFlagTwo = false;
} else {
// error.addError("unable_to_get_data", "could not get data from the Sick S300");
return false;
}
// LOG(trace) << "receiving range scan from Sick S300";
} catch (...) {
error.addError("unable_to_get_data", "could not get data from the Sick S300");
return false;
}
return true;
// Bouml preserved body end 000211E7
}
Scanner::Scanner(kstring name, Syntax &stx, Positions &pos, Errors &err)
// ----------------------------------------------------------------------------
// Open the file and make sure it's readable
// ----------------------------------------------------------------------------
: syntax(stx),
input(*new utf8_ifstream(name)),
tokenText(""),
textValue(""), realValue(0.0), intValue(0), base(10),
indents(), indent(0), indentChar(0),
position(0), lineStart(0),
positions(pos), errors(err),
caseSensitive(Options::options ? Options::options->case_sensitive : true),
checkingIndent(false), settingIndent(false),
hadSpaceBefore(false), hadSpaceAfter(false),
mustDeleteInput(true)
{
indents.push_back(0); // We start with an indent of 0
position = positions.OpenFile(name);
if (input.fail())
err.Log(Error("File $1 cannot be read: $2", position).
Arg(name).Arg(strerror(errno)));
// Skip UTF-8 BOM if present
if (input.get() != 0xEF)
input.unget();
else if (input.get() != 0xBB)
input.unget(), input.unget();
else if(input.get() != 0xBF)
input.unget(), input.unget(), input.unget();
}
int main(int argc, char * argv[]) {
(Logger::getInstance()).init();
HokuyoURGConfiguration config;
config.devicePath = "/dev/ttyACM0"; // Device path of the Sick LMS 2xx
config.baud = BAUD_115200;
config.scanAngleStart = 0 *radian;
config.scanAngleStop = 1.5 *radian;
HokuyoURG scanner;
Errors errors;
if (!scanner.setConfiguration(config, errors)) {
errors.printErrorsToConsole();
return -1;
}
LaserScannerTools tools;
// tools.plot_laserscanner_values(scanner, errors);
/*
* Uninitialize the device
*/
try {
scanner.close(errors);
} catch (...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}
bool SickS300::getConfiguration(LaserScannerConfiguration& configuration, Errors& error) {
// Bouml preserved body begin 000210E7
if (!this->open(error)) {
return false;
}
try {
configuration.vendor = "SICK";
configuration.product = "S300";
configuration.scanAngleStart = -135.0 / 180.0 * M_PI * radian;
configuration.scanAngleStop = 135.0 / 180.0 * M_PI * radian;
configuration.scanResolution = ((-configuration.scanAngleStart) + configuration.scanAngleStop) / (double) numberOfScanPoints;
LOG(trace) << "read Sick LMS configuration";
} catch (...) {
error.addError("unable_to_read_configuration", "could not get the configuration from the Sick S300");
return false;
}
return true;
// Bouml preserved body end 000210E7
}
Scanner::Scanner(std::istream &input,
Syntax &stx, Positions &pos, Errors &err,
kstring fileName)
// ----------------------------------------------------------------------------
// Open the file and make sure it's readable
// ----------------------------------------------------------------------------
: syntax(stx),
input(input),
tokenText(""),
textValue(""), realValue(0.0), intValue(0), base(10),
indents(), indent(0), indentChar(0),
position(0), lineStart(0),
positions(pos), errors(err),
caseSensitive(Options::options ? Options::options->case_sensitive : true),
checkingIndent(false), settingIndent(false),
hadSpaceBefore(false), hadSpaceAfter(false),
mustDeleteInput(false)
{
indents.push_back(0); // We start with an indent of 0
position = positions.OpenFile(fileName);
if (input.fail())
err.Log(Error("Input stream cannot be read: $1", position)
.Arg(strerror(errno)));
}
bool SickS300::open(Errors& error) {
// Bouml preserved body begin 00021367
if (this->isConnected) {
return true;
}
if (this->config->devicePath == "") {
error.addError("no_DevicePath", "the device path is not specified in the configuration");
this->isConnected = false;
return false;
}
{
boost::mutex::scoped_lock lock_it(mutexSickS300);
if (sickS300 != NULL) {
error.addError("still_Connected", "a previous connection was not closed correctly please close it again.");
this->isConnected = false;
return false;
}
this->sickS300 = new ScannerSickS300();
}
unsigned int desired_baud = 500000;
switch (this->config->baud) {
case BAUD_9600:
desired_baud = 9600;
LOG(trace) << "using 9600 baut to comunicate to Sick S300";
break;
case BAUD_19200:
desired_baud = 19200;
LOG(trace) << "using 19200 baut to comunicate to Sick S300";
break;
case BAUD_38400:
desired_baud = 38400;
LOG(trace) << "using 38400 baut to comunicate to Sick S300";
break;
case BAUD_500K:
desired_baud = 500000;
LOG(trace) << "using 500000 baut to comunicate to Sick S300";
break;
case BAUD_UNKNOWN:
desired_baud = 0;
break;
}
//Initialize the Sick S300
try {
{
boost::mutex::scoped_lock lock_it(mutexSickS300);
if (!sickS300->open(this->config->devicePath.c_str(), desired_baud)) {
throw "could not initilize Sick S300";
}
this->isConnected = true;
}
LOG(trace) << "connection to Sick S300 initialized";
stopThread = false;
threads.create_thread(boost::bind(&SickS300::receiveScan, this));
} catch (...) {
error.addError("Initialize_failed", "could not initilize Sick S300");
{
boost::mutex::scoped_lock lock_it(mutexSickS300);
this->isConnected = false;
delete sickS300;
sickS300 = NULL;
}
return false;
}
return true;
// Bouml preserved body end 00021367
}
bool SickS300::resetDevice(Errors& error) {
// Bouml preserved body begin 000212E7
error.addError("unable_to_reset_sick_s300", "could not reset the Sick S300");
return false;
// Bouml preserved body end 000212E7
}
// Pre-Processamento:
// Remove os comentarios e linhas em branco
// E coloca o codigo numa estrutura do tipo CodeLines
// Tambem verifica os labels e equs e ifs
void readAndPreProcess (const char* fileName) {
ifstream infile(fileName);
string line;
int textMemAddr = textStartAddress;
int dataMemAddr = 0;
int BSSMemAddr = 0;
stringstream tempSS;
CodeSection codeSection = NONE;
// Le linha a linha
for (int lineCount = 1; getline(infile, line); ++lineCount) {
// Troca virgulas por espaco
strReplace(line, ",", " ");
// Ignora linhas em branco
if (line.empty()) continue;
// Pega palavra a palavra de acordo com os espacos
istringstream iss(line);
string tempStr;
while (iss >> tempStr) {
if ("SECTION" == tempStr) {
string tempStr2;
iss >> tempStr2;
if ("TEXT" == tempStr2)
codeSection = TEXT;
else if ("DATA" == tempStr2)
codeSection = DATA;
codeLines[lineCount].push_back(tempStr);
codeLines[lineCount].push_back(tempStr2);
continue;
}
// Ignora comentarios
if (";" == tempStr.substr(0,1)) break;
// Desconsidera o caso (maiusculas/minusculas)
transform(tempStr.begin(), tempStr.end(), tempStr.begin(), ::toupper);
// Ve se eh um label / define
if (":" == tempStr.substr(tempStr.length() - 1, 1)) {
// Ve se ainda restam tokens na linha
if (iss.rdbuf()->in_avail() != 0) {
// Remove o ':'
tempStr = tempStr.substr(0, tempStr.length() - 1);
string tempStr2;
iss >> tempStr2;
// Ve se o proximo token eh EQU
if ("EQU" == tempStr2) {
string tempStr3;
iss >> tempStr3;
// Se define já existe
if (defines.find(tempStr3) != defines.end()){
tempSS << lineCount;
errors.push("ERRO NA LINHA " + tempSS.str() + ": EQU ja declarado.");
tempSS.str("");
} else {
// Coloca o valor do EQU na tabela de defines
defines[tempStr] = tempStr3;
}
// Se nao eh so um label
// Com algo a mais na mesma linha
} else {
if ( (labels.find(tempStr) != labels.end()) ||
(dataLabels.find(tempStr) != dataLabels.end()) ){
tempSS << lineCount;
errors.push("ERRO NA LINHA " + tempSS.str() + ": Label ja declarado.");
tempSS.str("");
} else {
// Adiciona na tabela de labels
if(codeSection == TEXT){
labels[tempStr] = textMemAddr;
} else if (codeSection == DATA) {
dataLabels[tempStr] = dataMemAddr;
dataMemAddr += 4;
}
}
// Adiciona endereco de memoria
if (instructions.find(tempStr2) != instructions.end())
textMemAddr += get<3>(instructions[tempStr2]);
// Adiciona os tokens ao vetor
codeLines[lineCount].push_back(tempStr+":");
codeLines[lineCount].push_back(tempStr2);
}
// Se nao eh um label "sozinho"
// Adiciona no vetor
} else {
void Reporter::printErrors(std::ostream& os, Errors& errors, const int verbose)
{
StrStream strstream;
strstream << "Summary:";
// Parsing (not really errors)
if (unlikely(errors.commentedLines))
{
strstream << "\n [" << errors.commentedLines << "] commented lines";
}
if (unlikely(errors.blankLines))
{
strstream << "\n [" << errors.blankLines << "] blank lines";
}
auto nbErrors = errors.nbErrors();
if (unlikely(nbErrors > 0))
{
strstream << "\nFound " << nbErrors << " error:";
// Parsing
if (unlikely(errors.corruptedMessages))
{
strstream << "\n [" << errors.corruptedMessages << "] corrupted messages";
}
if (unlikely(errors.IncompleteMessages))
{
strstream << "\n [" << errors.IncompleteMessages << "] incomplete messages";
}
if (unlikely(errors.wrongActions))
{
strstream << "\n [" << errors.wrongActions << "] wrong actions";
}
if (unlikely(errors.wrongSides))
{
strstream << "\n [" << errors.wrongSides << "] wrong sides";
}
if (unlikely(errors.negativeOrderIds))
{
strstream << "\n [" << errors.negativeOrderIds << "] negative orderIds";
}
if (unlikely(errors.negativeQuantities))
{
strstream << "\n [" << errors.negativeQuantities << "] negative quantities";
}
if (unlikely(errors.negativePrices))
{
strstream << "\n [" << errors.negativePrices << "] negative prices";
}
if (unlikely(errors.missingActions))
{
strstream << "\n [" << errors.missingActions << "] missing actions";
}
if (unlikely(errors.missingOrderIds))
{
strstream << "\n [" << errors.missingOrderIds << "] missing orderIds";
}
if (unlikely(errors.missingSides))
{
strstream << "\n [" << errors.missingSides << "] missing sides";
}
if (unlikely(errors.missingQuantities))
{
strstream << "\n [" << errors.missingQuantities << "] missing quantities";
}
if (unlikely(errors.missingPrices))
{
strstream << "\n [" << errors.missingPrices << "] missing prices";
}
if (unlikely(errors.zeroOrderIds))
{
strstream << "\n [" << errors.zeroOrderIds << "] zero orderIds";
}
if (unlikely(errors.zeroQuantities))
{
strstream << "\n [" << errors.zeroQuantities << "] zero quantities";
}
if (unlikely(errors.zeroPrices))
{
strstream << "\n [" << errors.zeroPrices << "] zero prices";
}
if (unlikely(errors.outOfBoundsOrderIds))
{
strstream << "\n [" << errors.outOfBoundsOrderIds << "] out of bounds orderIds";
}
if (unlikely(errors.outOfBoundsQuantities))
{
strstream << "\n [" << errors.outOfBoundsQuantities << "] out of bounds quantities";
}
if (unlikely(errors.outOfBoundsPrices))
{
strstream << "\n [" << errors.outOfBoundsPrices << "] out of bounds prices";
}
// Order Management
if (unlikely(errors.duplicateOrderIds))
{
strstream << "\n [" << errors.duplicateOrderIds << "] duplicate OrderIds";
}
if (unlikely(errors.modifiesWithUnknownOrderId))
{
strstream << "\n [" << errors.modifiesWithUnknownOrderId << "] modifies with unknown OrderIds";
}
if (unlikely(errors.modifiesNotMatchedPrice))
{
strstream << "\n [" << errors.modifiesNotMatchedPrice << "] modifies with not matched order price";
}
if (unlikely(errors.cancelsWithUnknownOrderId))
{
strstream << "\n [" << errors.cancelsWithUnknownOrderId << "] cancels with unknown OrderIds";
}
if (unlikely(errors.cancelsNotMatchedQtyOrPrice))
{
strstream << "\n [" << errors.cancelsNotMatchedQtyOrPrice << "] cancels with not matched order price";
}
if (unlikely(errors.bestBidEqualOrUpperThanBestAsk))
{
strstream << "\n [" << errors.bestBidEqualOrUpperThanBestAsk << "] best bid equal or upper than best ask";
}
}
else
{
strstream << "\nNo error found";
}
auto nbCriticalErrors = errors.nbCriticalErrors();
if (unlikely(nbCriticalErrors > 0))
{
strstream << "\nFound [" << nbCriticalErrors << "] critical error:";
if (unlikely(errors.modifiesLimitQtyTooLow))
{
strstream << "\n [" << errors.modifiesLimitQtyTooLow << "] modifies with limit quantity too low";
}
if (unlikely(errors.modifiesLimitNotFound))
{
strstream << "\n [" << errors.modifiesLimitNotFound << "] modifies limit not found";
}
if (unlikely(errors.cancelsLimitQtyTooLow))
{
strstream << "\n [" << errors.cancelsLimitQtyTooLow << "] cancels with limit quantity too low";
}
if (unlikely(errors.cancelsLimitNotFound))
{
strstream << "\n [" << errors.cancelsLimitNotFound << "] cancels limit not found";
}
}
else
{
strstream << "\nNo critical error found";
}
strstream << '\n';
if (unlikely(verbose))
strstream << "Summary length: " << strstream.length() << '\n';
os.rdbuf()->sputn(strstream.c_str(), strstream.length());
os.flush();
}
bool Errors::equals(const Errors& expected, double tol) const {
if( size() != expected.size() ) return false;
return equal(begin(),end(),expected.begin(),equalsVector(tol));
}
/* ************************************************************************* */
void print(const Errors& a, const string& s) {
a.print(s);
}
void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) {
Errors::const_iterator it = x.begin();
BOOST_FOREACH(Vector& yi, y)
axpy(alpha,*(it++),yi);
}
bool TypeInference::Unify(Tree *t1, Tree *t2, unify_mode mode)
// ----------------------------------------------------------------------------
// Unify two type forms
// ----------------------------------------------------------------------------
// A type form in XL can be:
// - A type name integer
// - A generic type name #ABC
// - A litteral value 0 1.5 "Hello"
// - A range of values 0..4 1.3..8.9 "A".."Z"
// - A union of types 0,3,5 integer|real
// - A block for precedence (real)
// - A rewrite specifier integer => real
// - The type of a pattern type (X:integer, Y:integer)
//
// Unification happens almost as "usual" for Algorithm W, except for how
// we deal with XL "shape-based" type constructors, e.g. type(P)
{
// Make sure we have the canonical form
t1 = Base(t1);
t2 = Base(t2);
if (t1 == t2)
return true; // Already unified
// Strip out blocks in type specification
if (Block *b1 = t1->AsBlock())
if (Unify(b1->child, t2))
return Join(b1, t2);
if (Block *b2 = t2->AsBlock())
if (Unify(t1, b2->child))
return Join(t1, b2);
// Lookup type names, replace them with their value
t1 = LookupTypeName(t1);
t2 = LookupTypeName(t2);
if (t1 == t2)
return true; // This may have been enough for unifiation
// Special case of constants
if (t1->IsConstant())
if (Name *t2n = t2->AsName())
return JoinConstant(t1, t2n);
if (t2->IsConstant())
if (Name *t1n = t1->AsName())
return JoinConstant(t2, t1n);
// If either is a generic, unify with the other
if (IsGeneric(t1))
return Join(t1, t2);
if (IsGeneric(t2))
return Join(t1, t2);
// Check if t1 is one of the infix constructor types
if (Infix *i1 = t1->AsInfix())
{
// Function types: Unifies only with a function type
if (i1->name == "=>")
{
if (Infix *i2 = t2->AsInfix())
if (i2->name == "=>")
return
Unify(i1->left, i2->left, i1, i2) &&
Unify(i1->right, i2->right, i1, i2);
return TypeError(i1, t2);
}
// Union types: Unify with either side
if (i1->name == "|" || i1->name == ",")
{
if (mode != DECLARATION)
{
Errors errors;
if (Unify(i1->left, t2))
return true;
errors.Swallowed();
if (Unify(i1->right, t2))
return true;
}
return TypeError(t1, t2);
}
Ooops("Malformed type definition $2 for $1", left, i1);
return false;
}
if (Infix *i2 = t2->AsInfix())
{
// Union types: Unify with either side
if (i2->name == "|" || i2->name == ",")
{
Errors errors;
if (Unify(t1, i2->left))
return true;
errors.Swallowed();
if (Unify(t1, i2->right))
return true;
return false;
}
Ooops("Malformed type definition $2 for $1", right, i2);
return false;
}
// If we have a type name at this stage, this is a failure
if (IsTypeName(t1))
{
// In declaration mode, we have success if t2 covers t1
if (mode == DECLARATION && TypeCoversType(context, t2, t1, false))
return true;
return TypeError(t1, t2);
}
if (IsTypeName(t2))
{
return TypeError(t1, t2);
}
// Check prefix constructor types
if (Prefix *p1 = t1->AsPrefix())
if (Name *pn1 = p1->left->AsName())
if (pn1->value == "type")
if (Prefix *p2 = t2->AsPrefix())
if (Name *pn2 = p2->right->AsName())
if (pn2->value == "type")
if (UnifyPatterns(p1->right, p2->right))
return Join(t1, t2);
// None of the above: fail
return TypeError(t1, t2);
}
bool TypeInference::Evaluate(Tree *what)
// ----------------------------------------------------------------------------
// Find candidates for the given expression and infer types from that
// ----------------------------------------------------------------------------
{
// We don't evaluate expressions while prototyping a pattern
if (prototyping)
return true;
// Record if we are matching patterns
bool matchingPattern = matching;
matching = false;
// Look directly inside blocks
while (Block *block = what->AsBlock())
what = block->child;
// Evaluating constants is always successful
if (what->IsConstant() && !context->hasConstants)
return AssignType(what, what);
// Test if we are already trying to evaluate this particular form
rcall_map::iterator found = rcalls.find(what);
bool recursive = found != rcalls.end();
if (recursive)
return true;
// Identify all candidate rewrites in the current context
RewriteCalls_p rc = new RewriteCalls(this);
rcalls[what] = rc;
uint count = 0;
ulong key = context->Hash(what);
Errors errors;
errors.Log (Error("Unable to evaluate $1 because", what), true);
context->Evaluate(what, *rc, key, Context::NORMAL_LOOKUP);
// If we have no candidate, this is a failure
count = rc->candidates.size();
if (count == 0)
{
if (matchingPattern && what->Kind() > KIND_LEAF_LAST)
{
Tree *wtype = Type(what);
return Unify(wtype, tree_type, what, what);
}
Ooops("No form matches $1", what);
return false;
}
errors.Clear();
errors.Log(Error("Unable to check types in $1 because", what), true);
// The resulting type is the union of all candidates
Tree *type = Base(rc->candidates[0].type);
Tree *wtype = Type(what);
for (uint i = 1; i < count; i++)
{
Tree *ctype = rc->candidates[i].type;
ctype = Base(ctype);
if (IsGeneric(ctype) && IsGeneric(wtype))
{
// foo:#A rewritten as bar:#B and another type
// Join types instead of performing a union
if (!Join(ctype, type))
return false;
if (!Join(wtype, type))
return false;
continue;
}
type = UnionType(context, type, ctype);
}
// Perform type unification
return Unify(type, wtype, what, what, DECLARATION);
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
multiplyInPlace(x, e.begin());
}
/* ************************************************************************* */
Errors Errors::operator-() const {
Errors result;
BOOST_FOREACH(const Vector& ai, *this)
result.push_back(-ai);
return result;
}
