<commit_before>/*
 * Copyright (c) 2003-2008, John Wiegley.  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * - 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.
 *
 * - Neither the name of New Artisans LLC 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.
 */

#include "op.h"
#include "scope.h"
#include "binary.h"

namespace ledger {

expr_t::ptr_op_t expr_t::op_t::compile(scope_t& scope)
{
  switch (kind) {
  case IDENT:
    if (ptr_op_t def = scope.lookup(as_ident())) {
      // Definitions are compiled at the point of definition, not the
      // point of use.
      return copy(def);
    }
    return this;

  default:
    break;
  }

  if (kind < TERMINALS)
    return this;

  ptr_op_t lhs(left()->compile(scope));
  ptr_op_t rhs(kind > UNARY_OPERATORS ? right()->compile(scope) : ptr_op_t());

  if (lhs == left() && (! rhs || rhs == right()))
    return this;

  ptr_op_t intermediate(copy(lhs, rhs));

  if (lhs->is_value() && (! rhs || rhs->is_value()))
    return wrap_value(intermediate->calc(scope));

  return intermediate;
}

value_t expr_t::op_t::calc(scope_t& scope)
{
  switch (kind) {
  case VALUE:
    return as_value();

  case IDENT:
    if (! left())
      throw_(calc_error, "Unknown identifier '" << as_ident() << "'");
    return left()->calc(scope);

  case FUNCTION: {
    // Evaluating a FUNCTION is the same as calling it directly; this happens
    // when certain functions-that-look-like-variables (such as "amount") are
    // resolved.
    call_scope_t call_args(scope);
    return as_function()(call_args);
  }

  case O_CALL: {
    call_scope_t call_args(scope);

    if (right())
      call_args.set_args(right()->calc(scope));

    ptr_op_t func = left();
    string   name;

    assert(func->kind == IDENT);
    func = func->left();

    if (func->kind != FUNCTION)
      throw_(calc_error, "Calling non-function");

    return func->as_function()(call_args);
  }

  case O_MATCH:
    assert(right()->is_mask());
    return right()->as_mask().match(left()->calc(scope).to_string());

  case INDEX: {
    const call_scope_t& args(downcast<const call_scope_t>(scope));

    if (as_index() < args.size())
      return args[as_index()];
    else
      throw_(calc_error, "Reference to non-existing argument " << as_index());
    break;
  }

  case O_NEQ:
    return left()->calc(scope) != right()->calc(scope);
  case O_EQ:
    return left()->calc(scope) == right()->calc(scope);
  case O_LT:
    return left()->calc(scope) <  right()->calc(scope);
  case O_LTE:
    return left()->calc(scope) <= right()->calc(scope);
  case O_GT:
    return left()->calc(scope) >  right()->calc(scope);
  case O_GTE:
    return left()->calc(scope) >= right()->calc(scope);

  case O_ADD:
    return left()->calc(scope) + right()->calc(scope);
  case O_SUB:
    return left()->calc(scope) - right()->calc(scope);
  case O_MUL:
    return left()->calc(scope) * right()->calc(scope);
  case O_DIV:
    return left()->calc(scope) / right()->calc(scope);

  case O_NEG:
    assert(! right());
    return left()->calc(scope).negate();

  case O_NOT:
    assert(! right());
    return ! left()->calc(scope);

  case O_AND:
    return ! left()->calc(scope) ? value_t(false) : right()->calc(scope);

  case O_OR:
    if (value_t temp = left()->calc(scope))
      return temp;
    else
      return right()->calc(scope);

  case O_COMMA: {
    value_t result(left()->calc(scope));

    ptr_op_t next = right();
    while (next) {
      ptr_op_t value_op;
      if (next->kind == O_COMMA /* || next->kind == O_UNION */) {
	value_op = next->left();
	next     = next->right();
      } else {
	value_op = next;
	next     = NULL;
      }

      result.push_back(value_op->calc(scope));
    }
    return result;
  }

  case LAST:
  default:
    assert(false);
    break;
  }

  return NULL_VALUE;
}

bool expr_t::op_t::print(std::ostream& out, print_context_t& context) const
{
  bool found = false;

  if (context.start_pos && this == context.op_to_find) {
    *context.start_pos = out.tellp();
    *context.start_pos--;
    found = true;
  }

  string symbol;

  switch (kind) {
  case VALUE: {
    as_value().print(out, context.relaxed);
    break;
  }

  case IDENT:
    out << as_ident();
    break;

  case FUNCTION:
    out << "<FUNCTION>";
    break;

  case INDEX:
    out << '@' << as_index();
    break;

  case O_NOT:
    out << "!";
    if (left() && left()->print(out, context))
      found = true;
    break;
  case O_NEG:
    out << "-";
    if (left() && left()->print(out, context))
      found = true;
    break;

  case O_ADD:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " + ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_SUB:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " - ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_MUL:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " * ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_DIV:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " / ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_NEQ:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " != ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_EQ:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " == ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_LT:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " < ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_LTE:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " <= ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_GT:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " > ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_GTE:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " >= ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_AND:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " & ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_OR:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " | ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_COMMA:
    if (left() && left()->print(out, context))
      found = true;
    out << ", ";
    if (right() && right()->print(out, context))
      found = true;
    break;

  case O_CALL:
    if (left() && left()->print(out, context))
      found = true;
    out << "(";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_MATCH:
    out << '/';
    if (left() && left()->print(out, context))
      found = true;
    out << "/ =~ ";
    if (right() && right()->print(out, context))
      found = true;
    break;

  case LAST:
  default:
    assert(false);
    break;
  }

  if (! symbol.empty()) {
    if (amount_t::current_pool->find(symbol))
      out << '@';
    out << symbol;
  }

  if (context.end_pos && this == context.op_to_find)
    *context.end_pos = static_cast<unsigned long>(out.tellp()) - 1;

  return found;
}

void expr_t::op_t::dump(std::ostream& out, const int depth) const
{
  out.setf(std::ios::left);
  out.width(10);
  out << this;

  for (int i = 0; i < depth; i++)
    out << " ";

  switch (kind) {
  case VALUE:
    out << "VALUE - " << as_value();
    break;

  case IDENT:
    out << "IDENT - " << as_ident();
    break;

  case INDEX:
    out << "INDEX - " << as_index();
    break;

  case FUNCTION:
    out << "FUNCTION";
    break;

  case O_CALL:	out << "O_CALL"; break;
  case O_MATCH:	out << "O_MATCH"; break;

  case O_NOT:	out << "O_NOT"; break;
  case O_NEG:	out << "O_NEG"; break;

  case O_ADD:	out << "O_ADD"; break;
  case O_SUB:	out << "O_SUB"; break;
  case O_MUL:	out << "O_MUL"; break;
  case O_DIV:	out << "O_DIV"; break;

  case O_NEQ:	out << "O_NEQ"; break;
  case O_EQ:	out << "O_EQ"; break;
  case O_LT:	out << "O_LT"; break;
  case O_LTE:	out << "O_LTE"; break;
  case O_GT:	out << "O_GT"; break;
  case O_GTE:	out << "O_GTE"; break;

  case O_AND:	out << "O_AND"; break;
  case O_OR:	out << "O_OR"; break;

  case O_COMMA:	out << "O_COMMA"; break;

  case LAST:
  default:
    assert(false);
    break;
  }

  out << " (" << refc << ')' << std::endl;

  // An identifier is a special non-terminal, in that its left() can
  // hold the compiled definition of the identifier.
  if (kind > TERMINALS || kind == IDENT) {
    if (left()) {
      left()->dump(out, depth + 1);
      if (right())
	right()->dump(out, depth + 1);
    } else {
      assert(! right());
    }
  }
}

void expr_t::op_t::read(const char *& data)
{
  kind = binary::read_long<kind_t>(data);

  if (kind > TERMINALS) {
    set_left(new expr_t::op_t());
    left()->read(data);

    if (binary::read_bool(data)) {
      set_right(new expr_t::op_t());
      right()->read(data);
    }
  }

  switch (kind) {
  case VALUE: {
    value_t temp;
    temp.read(data);
    set_value(temp);
    break;
  }
  case IDENT: {
    string temp;
    binary::read_string(data, temp);
    set_ident(temp);
    break;
  }
  case MASK: {
    mask_t temp;
    temp.read(data);
    set_mask(temp);
    break;
  }
  case INDEX: {
    long temp;
    binary::read_long(data, temp);
    set_index(temp);
    break;
  }

  default:
    assert(false);
    break;
  }
}

void expr_t::op_t::write(std::ostream& out) const
{
  binary::write_long<kind_t>(out, kind);

  if (kind > TERMINALS) {
    left()->write(out);
    if (right()) {
      binary::write_bool(out, true);
      right()->write(out);
    } else {
      binary::write_bool(out, false);
    }
  } else {
    switch (kind) {
    case VALUE:
      as_value().write(out);
      break;
    case IDENT:
      binary::write_string(out, as_ident());
      break;
    case MASK:
      as_mask().write(out);
      break;
    case INDEX:
      binary::write_long(out, as_index());
      break;

    case FUNCTION:
    default:
      assert(false);
      break;
    }
  }
}

} // namespace ledger
<commit_msg>Corrected several assertions which could occur when using unary operators and unresolved identifiers.<commit_after>/*
 * Copyright (c) 2003-2008, John Wiegley.  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 * - Redistributions of source code must retain the above copyright
 *   notice, this list of conditions and the following disclaimer.
 *
 * - 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.
 *
 * - Neither the name of New Artisans LLC 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.
 */

#include "op.h"
#include "scope.h"
#include "binary.h"

namespace ledger {

expr_t::ptr_op_t expr_t::op_t::compile(scope_t& scope)
{
  switch (kind) {
  case IDENT:
    if (ptr_op_t def = scope.lookup(as_ident())) {
      // Definitions are compiled at the point of definition, not the
      // point of use.
      return copy(def);
    }
    return this;

  default:
    break;
  }

  if (kind < TERMINALS)
    return this;

  ptr_op_t lhs(left()->compile(scope));
  ptr_op_t rhs(kind > UNARY_OPERATORS ? right()->compile(scope) : ptr_op_t());

  if (lhs == left() && (! rhs || rhs == right()))
    return this;

  ptr_op_t intermediate(copy(lhs, rhs));

  if (lhs->is_value() && (! rhs || rhs->is_value()))
    return wrap_value(intermediate->calc(scope));

  return intermediate;
}

value_t expr_t::op_t::calc(scope_t& scope)
{
  switch (kind) {
  case VALUE:
    return as_value();

  case IDENT:
    if (! left())
      throw_(calc_error, "Unknown identifier '" << as_ident() << "'");
    return left()->calc(scope);

  case FUNCTION: {
    // Evaluating a FUNCTION is the same as calling it directly; this happens
    // when certain functions-that-look-like-variables (such as "amount") are
    // resolved.
    call_scope_t call_args(scope);
    return as_function()(call_args);
  }

  case O_CALL: {
    call_scope_t call_args(scope);

    if (right())
      call_args.set_args(right()->calc(scope));

    ptr_op_t func = left();
    string   name;

    assert(func->kind == IDENT);
    func = func->left();

    if (func->kind != FUNCTION)
      throw_(calc_error, "Calling non-function");

    return func->as_function()(call_args);
  }

  case O_MATCH:
    assert(right()->is_mask());
    return right()->as_mask().match(left()->calc(scope).to_string());

  case INDEX: {
    const call_scope_t& args(downcast<const call_scope_t>(scope));

    if (as_index() < args.size())
      return args[as_index()];
    else
      throw_(calc_error, "Reference to non-existing argument " << as_index());
    break;
  }

  case O_NEQ:
    return left()->calc(scope) != right()->calc(scope);
  case O_EQ:
    return left()->calc(scope) == right()->calc(scope);
  case O_LT:
    return left()->calc(scope) <  right()->calc(scope);
  case O_LTE:
    return left()->calc(scope) <= right()->calc(scope);
  case O_GT:
    return left()->calc(scope) >  right()->calc(scope);
  case O_GTE:
    return left()->calc(scope) >= right()->calc(scope);

  case O_ADD:
    return left()->calc(scope) + right()->calc(scope);
  case O_SUB:
    return left()->calc(scope) - right()->calc(scope);
  case O_MUL:
    return left()->calc(scope) * right()->calc(scope);
  case O_DIV:
    return left()->calc(scope) / right()->calc(scope);

  case O_NEG:
    return left()->calc(scope).negate();

  case O_NOT:
    return ! left()->calc(scope);

  case O_AND:
    return ! left()->calc(scope) ? value_t(false) : right()->calc(scope);

  case O_OR:
    if (value_t temp = left()->calc(scope))
      return temp;
    else
      return right()->calc(scope);

  case O_COMMA: {
    value_t result(left()->calc(scope));

    ptr_op_t next = right();
    while (next) {
      ptr_op_t value_op;
      if (next->kind == O_COMMA /* || next->kind == O_UNION */) {
	value_op = next->left();
	next     = next->right();
      } else {
	value_op = next;
	next     = NULL;
      }

      result.push_back(value_op->calc(scope));
    }
    return result;
  }

  case LAST:
  default:
    assert(false);
    break;
  }

  return NULL_VALUE;
}

bool expr_t::op_t::print(std::ostream& out, print_context_t& context) const
{
  bool found = false;

  if (context.start_pos && this == context.op_to_find) {
    *context.start_pos = out.tellp();
    *context.start_pos--;
    found = true;
  }

  string symbol;

  switch (kind) {
  case VALUE: {
    as_value().print(out, context.relaxed);
    break;
  }

  case IDENT:
    out << as_ident();
    break;

  case FUNCTION:
    out << "<FUNCTION>";
    break;

  case INDEX:
    out << '@' << as_index();
    break;

  case O_NOT:
    out << "!";
    if (left() && left()->print(out, context))
      found = true;
    break;
  case O_NEG:
    out << "-";
    if (left() && left()->print(out, context))
      found = true;
    break;

  case O_ADD:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " + ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_SUB:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " - ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_MUL:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " * ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_DIV:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " / ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_NEQ:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " != ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_EQ:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " == ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_LT:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " < ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_LTE:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " <= ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_GT:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " > ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_GTE:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " >= ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_AND:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " & ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;
  case O_OR:
    out << "(";
    if (left() && left()->print(out, context))
      found = true;
    out << " | ";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_COMMA:
    if (left() && left()->print(out, context))
      found = true;
    out << ", ";
    if (right() && right()->print(out, context))
      found = true;
    break;

  case O_CALL:
    if (left() && left()->print(out, context))
      found = true;
    out << "(";
    if (right() && right()->print(out, context))
      found = true;
    out << ")";
    break;

  case O_MATCH:
    out << '/';
    if (left() && left()->print(out, context))
      found = true;
    out << "/ =~ ";
    if (right() && right()->print(out, context))
      found = true;
    break;

  case LAST:
  default:
    assert(false);
    break;
  }

  if (! symbol.empty()) {
    if (amount_t::current_pool->find(symbol))
      out << '@';
    out << symbol;
  }

  if (context.end_pos && this == context.op_to_find)
    *context.end_pos = static_cast<unsigned long>(out.tellp()) - 1;

  return found;
}

void expr_t::op_t::dump(std::ostream& out, const int depth) const
{
  out.setf(std::ios::left);
  out.width(10);
  out << this;

  for (int i = 0; i < depth; i++)
    out << " ";

  switch (kind) {
  case VALUE:
    out << "VALUE - " << as_value();
    break;

  case IDENT:
    out << "IDENT - " << as_ident();
    break;

  case INDEX:
    out << "INDEX - " << as_index();
    break;

  case FUNCTION:
    out << "FUNCTION";
    break;

  case O_CALL:	out << "O_CALL"; break;
  case O_MATCH:	out << "O_MATCH"; break;

  case O_NOT:	out << "O_NOT"; break;
  case O_NEG:	out << "O_NEG"; break;

  case O_ADD:	out << "O_ADD"; break;
  case O_SUB:	out << "O_SUB"; break;
  case O_MUL:	out << "O_MUL"; break;
  case O_DIV:	out << "O_DIV"; break;

  case O_NEQ:	out << "O_NEQ"; break;
  case O_EQ:	out << "O_EQ"; break;
  case O_LT:	out << "O_LT"; break;
  case O_LTE:	out << "O_LTE"; break;
  case O_GT:	out << "O_GT"; break;
  case O_GTE:	out << "O_GTE"; break;

  case O_AND:	out << "O_AND"; break;
  case O_OR:	out << "O_OR"; break;

  case O_COMMA:	out << "O_COMMA"; break;

  case LAST:
  default:
    assert(false);
    break;
  }

  out << " (" << refc << ')' << std::endl;

  // An identifier is a special non-terminal, in that its left() can
  // hold the compiled definition of the identifier.
  if (kind > TERMINALS || kind == IDENT) {
    if (left()) {
      left()->dump(out, depth + 1);
      if (kind > UNARY_OPERATORS && right())
	right()->dump(out, depth + 1);
    }
    else if (kind > UNARY_OPERATORS) {
      assert(! right());
    }
  }
}

void expr_t::op_t::read(const char *& data)
{
  kind = binary::read_long<kind_t>(data);

  if (kind > TERMINALS) {
    set_left(new expr_t::op_t());
    left()->read(data);

    if (kind > UNARY_OPERATORS && binary::read_bool(data)) {
      set_right(new expr_t::op_t());
      right()->read(data);
    }
  }

  switch (kind) {
  case VALUE: {
    value_t temp;
    temp.read(data);
    set_value(temp);
    break;
  }
  case IDENT: {
    string temp;
    binary::read_string(data, temp);
    set_ident(temp);
    break;
  }
  case MASK: {
    mask_t temp;
    temp.read(data);
    set_mask(temp);
    break;
  }
  case INDEX: {
    long temp;
    binary::read_long(data, temp);
    set_index(temp);
    break;
  }

  default:
    assert(false);
    break;
  }
}

void expr_t::op_t::write(std::ostream& out) const
{
  binary::write_long<kind_t>(out, kind);

  if (kind > TERMINALS) {
    left()->write(out);

    if (kind > UNARY_OPERATORS) {
      if (right()) {
	binary::write_bool(out, true);
	right()->write(out);
      } else {
	binary::write_bool(out, false);
      }
    }
  } else {
    switch (kind) {
    case VALUE:
      as_value().write(out);
      break;
    case IDENT:
      binary::write_string(out, as_ident());
      break;
    case MASK:
      as_mask().write(out);
      break;
    case INDEX:
      binary::write_long(out, as_index());
      break;

    case FUNCTION:
    default:
      assert(false);
      break;
    }
  }
}

} // namespace ledger
<|endoftext|>


<commit_before>/**
   MiracleGrue - Model Generator for toolpathing. <http://www.grue.makerbot.com>
   Copyright (C) 2011 Far McKon <Far@makerbot.com>, Hugo Boyer (hugo@makerbot.com)

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Affero General Public License as
   published by the Free Software Foundation, either version 3 of the
   License, or (at your option) any later version.

 */

#include <iostream>
#include <string>

#include <stdlib.h>
#include <stdint.h>

#include "mgl/abstractable.h"
#include "mgl/configuration.h"
#include "mgl/miracle.h"

#include "mgl/Vector2.h"
#include "optionparser.h"

#include "mgl/log.h"

using namespace std;
using namespace mgl;

/// Extends options::Arg to specifiy limitations on arguments

struct Arg : public option::Arg {

	static void printError(const char* msg1, const option::Option& opt, const char* msg2) {
		fprintf(stderr, "%s", msg1);
		fwrite(opt.name, opt.namelen, 1, stderr);
		fprintf(stderr, "%s", msg2);
	}

	static option::ArgStatus Unknown(const option::Option& option, bool msg) {
		if (msg) printError("Unknown option '", option, "'\n");
		return option::ARG_ILLEGAL;
	}

	static option::ArgStatus Required(const option::Option& option, bool msg) {
		if (option.arg != 0)
			return option::ARG_OK;

		if (msg) printError("Option '", option, "' requires an argument\n");
		return option::ARG_ILLEGAL;
	}

	static option::ArgStatus NonEmpty(const option::Option& option, bool msg) {
		if (option.arg != 0 && option.arg[0] != 0)
			return option::ARG_OK;

		if (msg) printError("Option '", option, "' requires a non-empty argument\n");
		return option::ARG_ILLEGAL;
	}

	static option::ArgStatus Numeric(const option::Option& option, bool msg) {
		char* endptr = 0;
		if (option.arg != 0 && strtod(option.arg, &endptr)) {
		};
		if (endptr != option.arg && *endptr == 0)
			return option::ARG_OK;

		if (msg) printError("Option '", option, "' requires a numeric argument\n");
		return option::ARG_ILLEGAL;
	}
};

// all ID's of the options we expect

enum optionIndex {
	UNKNOWN, HELP, CONFIG, FIRST_Z, LAYER_H, LAYER_W, FILL_ANGLE,
	FILL_DENSITY, N_SHELLS, BOTTOM_SLICE_IDX, TOP_SLICE_IDX,
	DEBUG_ME, DEBUG_LAYER, START_GCODE, END_GCODE,
	DEFAULT_EXTRUDER, OUT_FILENAME, JSON_PROGRESS
};
// options descriptor table
const option::Descriptor usageDescriptor[] ={
	{UNKNOWN, 0, "", "", Arg::None, "miracle-grue [OPTIONS] FILE.STL \n\n"
		"Options:"},
	{HELP, 0, "", "help", Arg::None, "  --help  \tPrint usage and exit."},
	{CONFIG, 1, "c", "config", Arg::NonEmpty, "-c  \tconfig data in a config.json file."
		"(default is local miracle.config)"},
	{FIRST_Z, 2, "f", "bedZOffset", Arg::Numeric,
		"-f \tfirst layer height (mm)"},
	{LAYER_H, 3, "h", "layerHeight", Arg::Numeric,
		"  -h \tgeneral layer height(mm)"},
	{LAYER_W, 4, "w", "layerWidth", Arg::Numeric,
		"  -w \tlayer width(mm)"},
	{ FILL_ANGLE, 5, "a", "angle", Arg::Numeric,
		"  -a \tinfill grid inter slice angle(radians)"},
	{ FILL_DENSITY, 6, "p", "infillDensity", Arg::Numeric,
		"  -p \tapprox infill density(percent), aka rho aka p"},
	{ N_SHELLS, 7, "n", "numberOfShells", Arg::Numeric,
		"  -n \tnumber of shells per layer"},
	{ BOTTOM_SLICE_IDX, 8, "b", "bottomIdx", Arg::Numeric,
		"  -b \tbottom slice index"},
	{ TOP_SLICE_IDX, 9, "t", "topIdx", Arg::Numeric,
		"  -t \ttop slice index"},
	{ DEBUG_ME, 10, "d", "debug", Arg::Numeric,
		"  -d \tdebug level, 0 to 99. 60 is 'info'"},
	{ DEBUG_LAYER, 11, "l", "printLayerMessages", Arg::None,
		"  -l \tinsert layer messages in gcode"},
	{ START_GCODE, 12, "s", "startGcode", Arg::NonEmpty,
		"  -s \tstart gcode file"},
	{ END_GCODE, 13, "e", "endGcode", Arg::NonEmpty,
		"  -e \tend gcode file"},
	{ DEFAULT_EXTRUDER, 14, "x", "defaultExtruder", Arg::Numeric,
		"  -x \tindex of extruder to use on a single material print (1 is lowest)"},
	{ OUT_FILENAME, 15, "o", "outFilename", Arg::NonEmpty,
		"  -o \twrite gcode to specific filename (defaults to <model>.gcode)"},
	{ JSON_PROGRESS, 16, "j", "jsonProgress", Arg::None,
	  "  -j \toutput progress as machine parsable JSON"},
	{0, 0, 0, 0, 0, 0},
};

void usage() {

	cout << endl;
	cout << "It is pitch black. You are likely to be eaten by a grue." << endl;
	cout << "You are using " << GRUE_PROGRAM_NAME << " version " << GRUE_VERSION << endl;
	cout << endl;
	cout << "This program translates a 3d model file in STL format to GCODE toolpath for a " << endl;
	cout << "3D printer." << " Another fine MakerBot Industries product!" << endl;
	cout << endl;
	option::printUsage(std::cout, usageDescriptor);
	Log::severe() << " Log level::severe ";
	Log::info() << "::info";
	Log::fine() << "::fine";
	Log::finer() << "::finer";
	Log::finest() << "::finest";
	cout << endl;
}

int newParseArgs(Configuration &config,
		int argc, char *argv[],
		string &modelFile,
		int &firstSliceIdx,
		int &lastSliceIdx,
                 bool &jsonProgress,
                 int &slicenum) {

	string configFilename = "";
	jsonProgress = false;

	argc -= (argc > 0);
	argv += (argc > 0); // skip program name argv[0] if present
	option::Stats stats(usageDescriptor, argc, argv);
	option::Option* options = new option::Option[stats.options_max];
	option::Option* buffer = new option::Option[stats.buffer_max];
	option::Parser parse(usageDescriptor, argc, argv, options, buffer);

	if (parse.error())
		return -20;

	if (options[HELP] || argc == 0) {
		usage();
		exit(0);
	}

	///read config file and/or help option first
	if (options[CONFIG]) {
		configFilename = string(options[CONFIG].arg);
	}

	// fallback to default config
	if (configFilename.compare(string("")) == 0)
		config.readFromDefault();
	else
		config.readFromFile(configFilename);

	for (int i = 0; i < parse.optionsCount(); ++i) {
		option::Option& opt = buffer[i];
		fprintf(stdout, "Argument #%d name %s is #%s\n", i, opt.desc->longopt, opt.arg);
		switch (opt.index()) {
		case LAYER_H:
			config[opt.desc->longopt] = atof(opt.arg);
			break;
		case LAYER_W:
		case FILL_ANGLE:
		case FILL_DENSITY:
            break;
		case N_SHELLS:
			config[opt.desc->longopt] = atoi(opt.arg);
			break;
		case BOTTOM_SLICE_IDX:
		case TOP_SLICE_IDX:
            break;
		case FIRST_Z:
			config[opt.desc->longopt] = atof(opt.arg);
			break;
		case DEBUG_ME:
			config["meta"][opt.desc->longopt] = atof(opt.arg);
			break;
		case DEBUG_LAYER:
			config[opt.desc->longopt] = true;
			break;
		case START_GCODE:
		case END_GCODE:
            config[opt.desc->longopt] = opt.arg;
			break;
		case DEFAULT_EXTRUDER:
			config[opt.desc->longopt] = atoi(opt.arg);
			break;
		case OUT_FILENAME:
			config[opt.desc->longopt] = opt.arg;
			break;
		case JSON_PROGRESS:
			jsonProgress = true;
                        config[opt.desc->longopt] = true;
			break;
		case CONFIG:
			// handled above before other config values
			break;
		case HELP:
			// not possible, because handled further above and exits the program
		default:
			break;
		}
	}

	/// handle parameters (not options!)
	if (parse.nonOptionsCount() == 0) {
		usage();
	} else if (parse.nonOptionsCount() != 2) {
		Log::severe() << "too many parameters" << endl;
		for (int i = 0; i < parse.nonOptionsCount(); ++i)
			Log::severe() << "Parameter #" << i << ": " << parse.nonOption(i) << "\n";
		exit(-10);
	} else {
		//handle the unnamed parameter separately
		modelFile = parse.nonOption(0);
		Log::finer() << "filename " << modelFile << endl;
		ifstream testmodel(modelFile.c_str(), ifstream::in);
		if (testmodel.fail()) {
			usage();
			throw mgl::Exception(("Invalid model file [" + modelFile + "]").c_str());
			exit(-10);
		}

        slicenum = atoi(parse.nonOption(1));
	}

	firstSliceIdx = -1;
	lastSliceIdx = -1;

	// [programName] and [versionStr] are always hard-code overwritten
	config["programName"] = GRUE_PROGRAM_NAME;
	config["versionStr"] = GRUE_VERSION;
	config["firmware"] = "unknown";

	if (false == config.isMember("machineName")) {
		config["machineName"] = "Machine Name Unknown";
	}

	/// convert debug data to a module/level specific setting
	g_debugVerbosity = log_verbosity_unset;
	if (config["meta"].isMember("debug")) {
		try {
			uint32_t debugLvl = config["meta"]["debug"].asUInt();
			if (debugLvl < 90) g_debugVerbosity = log_finest;
			else if (debugLvl < 80) g_debugVerbosity = log_finer;
			else if (debugLvl < 70) g_debugVerbosity = log_fine;
			else if (debugLvl < 60) g_debugVerbosity = log_info;
			else if (debugLvl < 10) g_debugVerbosity = log_severe;
			else g_debugVerbosity = log_verbosity_unset;
		}		catch (...) {
			cout << "fail sauce on debug level" << endl;
			// passed -d sans option. Assume default dbg level
			//g_debugVerbosity = log_default_level;
		}
	}

	return 0;
}

int main(int argc, char *argv[], char *[]) // envp
{

	string modelFile;
        bool jsonProgress = false;
	Configuration config;
	try {
		int firstSliceIdx, lastSliceIdx, slicenum;

		int ret = newParseArgs(config, argc, argv, modelFile, firstSliceIdx, lastSliceIdx, jsonProgress, slicenum);

		if (ret != 0) {
			usage();
			exit(ret);
		}

		// cout << config.asJson() << endl;

		MyComputer computer;
		Log::fine() << endl << endl;
		Log::fine() << "behold!" << endl;
		Log::fine() << "Materialization of \"" << modelFile << "\" has begun at " << computer.clock.now() << endl;

		std::string jsonFile = config["outFilename"].asString();

		if (jsonFile.empty()) {
			jsonFile = ".";
			jsonFile += computer.fileSystem.getPathSeparatorCharacter();
			jsonFile = computer.fileSystem.ChangeExtension(computer.fileSystem.ExtractFilename(modelFile.c_str()).c_str(), ".json");
		}

        GrueConfig grueCfg;
        grueCfg.loadFromFile(config);

		std::ofstream jsonFileStream;
        jsonFileStream.open(jsonFile.c_str(), ios::out);
        if(!jsonFileStream) {
            Exception mixup(std::string("Bad output file: ") +
                    jsonFile);
            throw mixup;
        }

		ProgressBar *log;
		if (jsonProgress) {
			log = new ProgressJSONStreamTotal();
		}
		else {
			log = new ProgressLog();
		}

		getSliceJson(grueCfg,
                     modelFile,
                     jsonFileStream,
                     slicenum);

		jsonFileStream.close();

		delete log;
	} catch (mgl::Exception &mixup) {
            if(jsonProgress) {
                exceptionToJson(Log::severe(), mixup, false);
            } else {
            	Log::severe() << "ERROR: " << mixup.error << endl;
            }
            return -1;
	} catch (char const* c) {
            if(jsonProgress) {
                exceptionToJson(Log::severe(), std::string(c), false);
            } else {
		Log::severe() << c << endl;
            }
            return -1;
	}

	exit(EXIT_SUCCESS);
}
<commit_msg>Fix bad get_slice<commit_after>/**
   MiracleGrue - Model Generator for toolpathing. <http://www.grue.makerbot.com>
   Copyright (C) 2011 Far McKon <Far@makerbot.com>, Hugo Boyer (hugo@makerbot.com)

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Affero General Public License as
   published by the Free Software Foundation, either version 3 of the
   License, or (at your option) any later version.

 */

#include <iostream>
#include <string>

#include <stdlib.h>
#include <stdint.h>

#include "mgl/abstractable.h"
#include "mgl/configuration.h"
#include "mgl/miracle.h"

#include "mgl/Vector2.h"
#include "optionparser.h"

#include "mgl/log.h"

using namespace std;
using namespace mgl;

/// Extends options::Arg to specifiy limitations on arguments

struct Arg : public option::Arg {

	static void printError(const char* msg1, const option::Option& opt, const char* msg2) {
		fprintf(stderr, "%s", msg1);
		fwrite(opt.name, opt.namelen, 1, stderr);
		fprintf(stderr, "%s", msg2);
	}

	static option::ArgStatus Unknown(const option::Option& option, bool msg) {
		if (msg) printError("Unknown option '", option, "'\n");
		return option::ARG_ILLEGAL;
	}

	static option::ArgStatus Required(const option::Option& option, bool msg) {
		if (option.arg != 0)
			return option::ARG_OK;

		if (msg) printError("Option '", option, "' requires an argument\n");
		return option::ARG_ILLEGAL;
	}

	static option::ArgStatus NonEmpty(const option::Option& option, bool msg) {
		if (option.arg != 0 && option.arg[0] != 0)
			return option::ARG_OK;

		if (msg) printError("Option '", option, "' requires a non-empty argument\n");
		return option::ARG_ILLEGAL;
	}

	static option::ArgStatus Numeric(const option::Option& option, bool msg) {
		char* endptr = 0;
		if (option.arg != 0 && strtod(option.arg, &endptr)) {
		};
		if (endptr != option.arg && *endptr == 0)
			return option::ARG_OK;

		if (msg) printError("Option '", option, "' requires a numeric argument\n");
		return option::ARG_ILLEGAL;
	}
};

// all ID's of the options we expect

enum optionIndex {
	UNKNOWN, HELP, CONFIG, FIRST_Z, LAYER_H, LAYER_W, FILL_ANGLE,
	FILL_DENSITY, N_SHELLS, BOTTOM_SLICE_IDX, TOP_SLICE_IDX,
	DEBUG_ME, DEBUG_LAYER, START_GCODE, END_GCODE,
	DEFAULT_EXTRUDER, OUT_FILENAME, JSON_PROGRESS
};
// options descriptor table
const option::Descriptor usageDescriptor[] ={
	{UNKNOWN, 0, "", "", Arg::None, "miracle-grue [OPTIONS] FILE.STL \n\n"
		"Options:"},
	{HELP, 0, "", "help", Arg::None, "  --help  \tPrint usage and exit."},
	{CONFIG, 1, "c", "config", Arg::NonEmpty, "-c  \tconfig data in a config.json file."
		"(default is local miracle.config)"},
	{FIRST_Z, 2, "f", "bedZOffset", Arg::Numeric,
		"-f \tfirst layer height (mm)"},
	{LAYER_H, 3, "h", "layerHeight", Arg::Numeric,
		"  -h \tgeneral layer height(mm)"},
	{LAYER_W, 4, "w", "layerWidth", Arg::Numeric,
		"  -w \tlayer width(mm)"},
	{ FILL_ANGLE, 5, "a", "angle", Arg::Numeric,
		"  -a \tinfill grid inter slice angle(radians)"},
	{ FILL_DENSITY, 6, "p", "infillDensity", Arg::Numeric,
		"  -p \tapprox infill density(percent), aka rho aka p"},
	{ N_SHELLS, 7, "n", "numberOfShells", Arg::Numeric,
		"  -n \tnumber of shells per layer"},
	{ BOTTOM_SLICE_IDX, 8, "b", "bottomIdx", Arg::Numeric,
		"  -b \tbottom slice index"},
	{ TOP_SLICE_IDX, 9, "t", "topIdx", Arg::Numeric,
		"  -t \ttop slice index"},
	{ DEBUG_ME, 10, "d", "debug", Arg::Numeric,
		"  -d \tdebug level, 0 to 99. 60 is 'info'"},
	{ DEBUG_LAYER, 11, "l", "printLayerMessages", Arg::None,
		"  -l \tinsert layer messages in gcode"},
	{ START_GCODE, 12, "s", "startGcode", Arg::NonEmpty,
		"  -s \tstart gcode file"},
	{ END_GCODE, 13, "e", "endGcode", Arg::NonEmpty,
		"  -e \tend gcode file"},
	{ DEFAULT_EXTRUDER, 14, "x", "defaultExtruder", Arg::Numeric,
		"  -x \tindex of extruder to use on a single material print (1 is lowest)"},
	{ OUT_FILENAME, 15, "o", "outFilename", Arg::NonEmpty,
		"  -o \twrite gcode to specific filename (defaults to <model>.gcode)"},
	{ JSON_PROGRESS, 16, "j", "jsonProgress", Arg::None,
	  "  -j \toutput progress as machine parsable JSON"},
	{0, 0, 0, 0, 0, 0},
};

void usage() {

	cout << endl;
	cout << "It is pitch black. You are likely to be eaten by a grue." << endl;
	cout << "You are using " << GRUE_PROGRAM_NAME << " version " << GRUE_VERSION << endl;
	cout << endl;
	cout << "This program translates a 3d model file in STL format to GCODE toolpath for a " << endl;
	cout << "3D printer." << " Another fine MakerBot Industries product!" << endl;
	cout << endl;
	option::printUsage(std::cout, usageDescriptor);
	Log::severe() << " Log level::severe ";
	Log::info() << "::info";
	Log::fine() << "::fine";
	Log::finer() << "::finer";
	Log::finest() << "::finest";
	cout << endl;
}

int newParseArgs(Configuration &config,
		int argc, char *argv[],
		string &modelFile,
		int &firstSliceIdx,
		int &lastSliceIdx,
                 bool &jsonProgress,
                 int &slicenum) {

	string configFilename = "";
	jsonProgress = false;

	argc -= (argc > 0);
	argv += (argc > 0); // skip program name argv[0] if present
	option::Stats stats(usageDescriptor, argc, argv);
	option::Option* options = new option::Option[stats.options_max];
	option::Option* buffer = new option::Option[stats.buffer_max];
	option::Parser parse(usageDescriptor, argc, argv, options, buffer);

	if (parse.error())
		return -20;

	if (options[HELP] || argc == 0) {
		usage();
		exit(0);
	}

	///read config file and/or help option first
	if (options[CONFIG]) {
		configFilename = string(options[CONFIG].arg);
	}

	// fallback to default config
	if (configFilename.compare(string("")) == 0)
		config.readFromDefault();
	else
		config.readFromFile(configFilename);

	for (int i = 0; i < parse.optionsCount(); ++i) {
		option::Option& opt = buffer[i];
		fprintf(stdout, "Argument #%d name %s is #%s\n", i, opt.desc->longopt, opt.arg);
		switch (opt.index()) {
		case LAYER_H:
			config[opt.desc->longopt] = atof(opt.arg);
			break;
		case LAYER_W:
		case FILL_ANGLE:
		case FILL_DENSITY:
            break;
		case N_SHELLS:
			config[opt.desc->longopt] = atoi(opt.arg);
			break;
		case BOTTOM_SLICE_IDX:
		case TOP_SLICE_IDX:
            break;
		case FIRST_Z:
			config[opt.desc->longopt] = atof(opt.arg);
			break;
		case DEBUG_ME:
			config["meta"][opt.desc->longopt] = atof(opt.arg);
			break;
		case DEBUG_LAYER:
			config[opt.desc->longopt] = true;
			break;
		case START_GCODE:
		case END_GCODE:
            config[opt.desc->longopt] = opt.arg;
			break;
		case DEFAULT_EXTRUDER:
			config[opt.desc->longopt] = atoi(opt.arg);
			break;
		case OUT_FILENAME:
			config[opt.desc->longopt] = opt.arg;
			break;
		case JSON_PROGRESS:
			jsonProgress = true;
                        config[opt.desc->longopt] = true;
			break;
		case CONFIG:
			// handled above before other config values
			break;
		case HELP:
			// not possible, because handled further above and exits the program
		default:
			break;
		}
	}

	/// handle parameters (not options!)
	if (parse.nonOptionsCount() == 0) {
		usage();
	} else if (parse.nonOptionsCount() != 2) {
		Log::severe() << "too many parameters" << endl;
		for (int i = 0; i < parse.nonOptionsCount(); ++i)
			Log::severe() << "Parameter #" << i << ": " << parse.nonOption(i) << "\n";
		exit(-10);
	} else {
		//handle the unnamed parameter separately
		modelFile = parse.nonOption(0);
		Log::finer() << "filename " << modelFile << endl;
		ifstream testmodel(modelFile.c_str(), ifstream::in);
		if (testmodel.fail()) {
			usage();
			throw mgl::Exception(("Invalid model file [" + modelFile + "]").c_str());
			exit(-10);
		}

        slicenum = atoi(parse.nonOption(1));
	}

	firstSliceIdx = -1;
	lastSliceIdx = -1;

	// [programName] and [versionStr] are always hard-code overwritten
	config["programName"] = GRUE_PROGRAM_NAME;
	config["versionStr"] = GRUE_VERSION;
	config["firmware"] = "unknown";

	if (false == config.isMember("machineName")) {
		config["machineName"] = "Machine Name Unknown";
	}

	/// convert debug data to a module/level specific setting
	g_debugVerbosity = log_verbosity_unset;
	if (config["meta"].isMember("debug")) {
		try {
			uint32_t debugLvl = config["meta"]["debug"].asUInt();
			if (debugLvl < 90) g_debugVerbosity = log_finest;
			else if (debugLvl < 80) g_debugVerbosity = log_finer;
			else if (debugLvl < 70) g_debugVerbosity = log_fine;
			else if (debugLvl < 60) g_debugVerbosity = log_info;
			else if (debugLvl < 10) g_debugVerbosity = log_severe;
			else g_debugVerbosity = log_verbosity_unset;
		}		catch (...) {
			cout << "fail sauce on debug level" << endl;
			// passed -d sans option. Assume default dbg level
			//g_debugVerbosity = log_default_level;
		}
	}

	return 0;
}

int main(int argc, char *argv[], char *[]) // envp
{

	string modelFile;
        bool jsonProgress = false;
	Configuration config;
	try {
		int firstSliceIdx, lastSliceIdx, slicenum;

		int ret = newParseArgs(config, argc, argv, modelFile, firstSliceIdx, lastSliceIdx, jsonProgress, slicenum);

		if (ret != 0) {
			usage();
			exit(ret);
		}

		// cout << config.asJson() << endl;

		MyComputer computer;
		Log::fine() << endl << endl;
		Log::fine() << "behold!" << endl;
		Log::fine() << "Materialization of \"" << modelFile << "\" has begun at " << computer.clock.now() << endl;

		std::string jsonFile = config["outFilename"].asString();

		if (jsonFile.empty()) {
			jsonFile = ".";
			jsonFile += computer.fileSystem.getPathSeparatorCharacter();
			jsonFile = computer.fileSystem.ChangeExtension(computer.fileSystem.ExtractFilename(modelFile.c_str()).c_str(), ".json");
		}

        GrueConfig grueCfg;
        grueCfg.loadFromFile(config);

		std::ofstream jsonFileStream;
        jsonFileStream.open(jsonFile.c_str(), ios::out);
        if(!jsonFileStream) {
            Exception mixup(std::string("Bad output file: ") +
                    jsonFile);
            throw mixup;
        }

		ProgressBar *log;
		if (jsonProgress) {
			log = new ProgressJSONStreamTotal(grueCfg);
		}
		else {
			log = new ProgressLog();
		}

		getSliceJson(grueCfg,
                     modelFile,
                     jsonFileStream,
                     slicenum);

		jsonFileStream.close();

		delete log;
	} catch (mgl::Exception &mixup) {
            if(jsonProgress) {
                exceptionToJson(Log::severe(), mixup, false);
            } else {
            	Log::severe() << "ERROR: " << mixup.error << endl;
            }
            return -1;
	} catch (char const* c) {
            if(jsonProgress) {
                exceptionToJson(Log::severe(), std::string(c), false);
            } else {
		Log::severe() << c << endl;
            }
            return -1;
	}

	exit(EXIT_SUCCESS);
}
<|endoftext|>


<commit_before>#include <iostream>

class Number
{

private:
    int number;

    std::vector<int> function getFactors();
    int function getLargestPrimeFactor();
}

#include <iostream>
#include <vector>
#include <cmath>

// Initializing
Number::Number(int n)
: mNumber(n)
{
}

// Get the factors of a number
std::vector<int> problem3::getFactors()
{
   double squareRootValue = floor(sqrt(mNumber));

   // Cheching if
   if(squareRootValue >= 2)
   {
       for(int i = 2; i <= squareRootValue; i++)
       {
           if(mNumber % i == 0)
               mFactors = push(i);
           else
               getFactors();
       }
   }
}

int problem3::getLargestPrimeFactor()
{

}

int main() {

	return 0;
}
<commit_msg>cleaning up<commit_after>#include <iostream>
#include <vector>
#include <cmath>

// Initializing
Number::Number(int n)
: mNumber(n)
{
}

// Get the factors of a number
std::vector<int> problem3::getFactors()
{
   double squareRootValue = floor(sqrt(mNumber));

   // Cheching if
   if(squareRootValue >= 2)
   {
       for(int i = 2; i <= squareRootValue; i++)
       {
           if(mNumber % i == 0)
               mFactors = push(i);
           else
               getFactors();
       }
   }
}

int problem3::getLargestPrimeFactor()
{

}

int main() {

	return 0;
}
<|endoftext|>


<commit_before>#include <ros/ros.h>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <std_msgs/Bool.h>
#include <std_msgs/UInt16MultiArray.h>

// custom message
#include <drv_msgs/recognized_target.h>

// STL
#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <dirent.h>

// OpenCV
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "kcftracker.hpp"
#include "utilities.h"

using namespace std;
using namespace cv;

const int angle_step = 1;
const float x_to_angle = 0.02; // a reasonable speed
const float y_to_angle = 0.02;

// Wait 50 loops befor target lost
#define WAIT_LOOP 50
int delay_ = WAIT_LOOP;

// Publishers
image_transport::Publisher trackPubImage_;
ros::Publisher trackPubStatus_;
ros::Publisher trackPubServo_;
ros::Publisher trackPubTargetLocation_;

// Image temps
cv_bridge::CvImageConstPtr src_;
cv::Mat src_img_;

cv_bridge::CvImagePtr track_ptr_(new cv_bridge::CvImage());
cv::Mat track_img_;

// Run mode
enum ModeType{m_wander, m_search, m_track};
int modeType_ = m_wander;
int modeTypeTemp_ = m_wander;
string param_running_mode = "/status/running_mode";

bool isInTracking_ = true;

// Target infomation
std_msgs::String tgt_label_;
cv::Rect roi_init_;

// Global params that record servo angle status
string param_servo_pitch = "/status/servo/pitch";
string param_servo_yaw = "/status/servo/yaw";
int pitch_ = 70;
int yaw_ = 90;

// Initialize the tracker
KCFTracker tracker; // use default settings

void publishServo(int pitch_angle, int yaw_angle)
{
  std_msgs::UInt16MultiArray array;
  array.data.push_back(pitch_angle);
  array.data.push_back(yaw_angle);
  pitch_ = pitch_angle;
  yaw_ = yaw_angle;
  trackPubServo_.publish(array);
}

void servoCallback(const std_msgs::UInt16MultiArrayConstPtr &msg)
{
  // this callback should always active
  pitch_ = msg->data[0];
  yaw_ = msg->data[1];
}

void resultCallback(const drv_msgs::recognized_targetConstPtr &msg)
{
  tgt_label_ = msg->label;
  int min_x = msg->tgt_bbox_array.data[0];
  int min_y = msg->tgt_bbox_array.data[1];
  int max_x = msg->tgt_bbox_array.data[2];
  int max_y = msg->tgt_bbox_array.data[3];

  roi_init_ = cv::Rect(min_x, min_y, max_x - min_x, max_y - min_y);
  tracker.initialized_ = false;
}

bool verifyDetection(Rect roi) {
  if (roi.area() < 20) {
    ROS_WARN("Target area in image is %d, too small to be tracked.\n", roi.area());
    return false;
  }
  if (roi.x < 0 || roi.x >= 640) {
    ROS_WARN("ROI X is %d.\n", roi.x);
    return false;
  }
  if (roi.y < 0 || roi.y >= 480) {
    ROS_WARN("ROI Y is %d.\n", roi.y);
    return false;
  }
  if (roi.x + roi.width >= 640) {
    ROS_WARN("ROI X+W is %d.\n", roi.x + roi.width);
    return false;
  }
  if (roi.y + roi.height >= 480) {
    ROS_WARN("ROI Y+H is %d.\n", roi.y + roi.height);
    return false;
  }
  if (roi.width <= 0 || roi.height <= 0) {
    ROS_WARN("ROI W, H is %d, %d.\n", roi.width, roi.height);
    return false;
  }
  return true;
}

void pubTarget(std_msgs::Header header, vector<unsigned int> mask_id, Rect roi) {
  // publish new target info
  drv_msgs::recognized_target result;
  result.header = header;
  result.label = tgt_label_;
  result.tgt_pixels.data = mask_id; // the datatype is uint 32 aka unsigned int
  result.tgt_bbox_array.data.push_back(roi.x);
  result.tgt_bbox_array.data.push_back(roi.y);
  result.tgt_bbox_array.data.push_back(roi.x + roi.width);
  result.tgt_bbox_array.data.push_back(roi.y + roi.height);
  result.tgt_bbox_center.data.push_back(roi.x + roi.width / 2);
  result.tgt_bbox_center.data.push_back(roi.y + roi.height / 2);

  trackPubTargetLocation_.publish(result);
}

bool postProcess(Rect roi, Mat &track_img)
{
  if (!verifyDetection(roi))
    return false;

  if (roi.area() < MIN_OBJECT_AREA || roi.area() > MAX_OBJECT_AREA)
    return false;

  Utilities::markImage(roi, track_img);
  return true;
}

void imageCallback(const sensor_msgs::ImageConstPtr& image_msg)
{
  if (modeType_ != m_track)
    return;

  if (!verifyDetection(roi_init_)) {
    isInTracking_ = false;
    tracker.initialized_ = false;
    return;
  }

  src_ = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8);
  src_img_ = src_->image;
  src_img_.copyTo(track_img_);

  // cv::rectangle(track_image_, detection_, cv::Scalar(232,228,53), 2);
  // Adjust the camera view based on ROI center
  if (!tracker.initialized_) {
    tracker.init(roi_init_, src_img_);
    tracker.initialized_ = true;
  }
  else {
    Rect roi = tracker.update(src_img_);

    vector<unsigned int> mask_id; // store object pixels id in image
    if (!postProcess(roi, track_img_)) {
      delay_--;
      if (delay_ < 0) {
        isInTracking_ = false;
        tracker.initialized_ = false;
      }
      return;
    }

    // publish image of target with bounding box
    track_ptr_->header = image_msg->header;
    track_ptr_->image = track_img_;
    track_ptr_->encoding = sensor_msgs::image_encodings::BGR8;
    trackPubImage_.publish(track_ptr_->toImageMsg());

    // drive the camera so that the center of the image captured is on object center
    int d_x = roi.x + roi.width / 2 - 320;
    int d_y = roi.y + roi.height / 2 - 240;
    int deg_x = int(d_x * x_to_angle); // offset the robot head
    int deg_y = int(d_y * y_to_angle);

    isInTracking_ = true;
    delay_ = WAIT_LOOP;
    pubTarget(image_msg->header, mask_id, roi);

    if (abs(deg_x) < angle_step && abs(deg_y) < angle_step) {
      // target on image center, continuing tracking..
      isInTracking_ = true;
      delay_ = WAIT_LOOP;
      pubTarget(image_msg->header, mask_id, roi);
      return;
    }
    if (abs(deg_x) >= angle_step && abs(deg_y) < angle_step) {
      // need move in x direction
      if (deg_x < -angle_step) deg_x = -angle_step;
      if (deg_x > angle_step) deg_x = angle_step;
      deg_y = 0;
    }
    if (abs(deg_x) < angle_step && abs(deg_y) >= angle_step) {
      // need move in y direction
      if (deg_y < -angle_step) deg_y = -angle_step;
      if (deg_y > angle_step) deg_y = angle_step;
      deg_x = 0;
    }
    if (abs(deg_x) >= angle_step && abs(deg_y) >= angle_step) {
      // need move in both directions
      if (deg_x < -angle_step) deg_x = -angle_step;
      if (deg_x > angle_step) deg_x = angle_step;
      if (deg_y < -angle_step) deg_y = -angle_step;
      if (deg_y > angle_step) deg_y = angle_step;
    }
    int x_ang = - deg_x + yaw_;
    int y_ang = - deg_y + pitch_;

    if (!(x_ang >= 0 && x_ang <= 180 && y_ang >= 60 && y_ang <= 140)) {
      // target center out of camera movable region
      pubTarget(image_msg->header, mask_id, roi);
      ROS_WARN_THROTTLE(31, "Target out of camera movable area.");
      // Although out of movable area, still in tracking
      // isInTracking_ = false;
      // tracker.initialized_ = false;
    }
    else {
      // target center is in camera movable region, so move the camera
      publishServo(y_ang, x_ang);
    }
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "drv_track_kcf");

  ros::NodeHandle nh;
  ros::NodeHandle pnh;
  ros::NodeHandle rgb_nh(nh, "rgb");
  ros::NodeHandle rgb_pnh(pnh, "rgb");

  image_transport::ImageTransport it_rgb_sub(rgb_nh);
  image_transport::TransportHints hints_rgb("compressed", ros::TransportHints(), rgb_pnh);

  image_transport::ImageTransport it_rgb_pub(nh);
  trackPubImage_ = it_rgb_pub.advertise("search/labeled_image", 1);
  trackPubServo_ = nh.advertise<std_msgs::UInt16MultiArray>("servo", 1);
  trackPubStatus_ = nh.advertise<std_msgs::Bool>("status/track/feedback", 1);
  trackPubTargetLocation_ = nh.advertise<drv_msgs::recognized_target>("track/recognized_target" , 1);

  ros::Subscriber sub_res = nh.subscribe<drv_msgs::recognized_target>("search/recognized_target", 1, resultCallback);
  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("image_rect_color", 1, imageCallback, hints_rgb);
  ros::Subscriber sub_s = nh.subscribe<std_msgs::UInt16MultiArray>("servo", 1, servoCallback);

  if (ros::param::has(param_servo_pitch))
    ros::param::get(param_servo_pitch, pitch_);

  if (ros::param::has(param_servo_yaw))
    ros::param::get(param_servo_yaw, yaw_);

  ROS_INFO("KCF tracking function initialized!\n");

  while (ros::ok())
  {
    if (ros::param::has(param_running_mode)) {
      ros::param::get(param_running_mode, modeTypeTemp_);

      if (modeTypeTemp_ != m_track && modeType_ == m_track)
        tracker.initialized_ = false;

      modeType_ = modeTypeTemp_;
    }

    std_msgs::Bool flag;
    flag.data = true;

    ros::spinOnce();

    flag.data = isInTracking_;
    trackPubStatus_.publish(flag);
  }

  return 0;
}
<commit_msg>Minor fix<commit_after>#include <ros/ros.h>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <std_msgs/Bool.h>
#include <std_msgs/UInt16MultiArray.h>

// custom message
#include <drv_msgs/recognized_target.h>

// STL
#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <dirent.h>

// OpenCV
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "kcftracker.hpp"
#include "utilities.h"

using namespace std;
using namespace cv;

const int angle_step = 1;
const float x_to_angle = 0.02; // a reasonable speed
const float y_to_angle = 0.02;

// Wait 50 loops befor target lost
#define WAIT_LOOP 50
int delay_ = WAIT_LOOP;

// Publishers
image_transport::Publisher trackPubImage_;
ros::Publisher trackPubStatus_;
ros::Publisher trackPubServo_;
ros::Publisher trackPubTargetLocation_;

// Image temps
cv_bridge::CvImageConstPtr src_;
Mat src_img_;

cv_bridge::CvImagePtr track_ptr_(new cv_bridge::CvImage());
Mat track_img_;

// Run mode
enum ModeType{m_wander, m_search, m_track};
int modeType_ = m_wander;
int modeTypeTemp_ = m_wander;
string param_running_mode = "/status/running_mode";

bool isInTracking_ = true;

// Target infomation
std_msgs::String tgt_label_;
Rect roi_init_;

// Global params that record servo angle status
string param_servo_pitch = "/status/servo/pitch";
string param_servo_yaw = "/status/servo/yaw";
int pitch_ = 70;
int yaw_ = 90;

// Initialize the tracker
KCFTracker tracker; // use default settings

void publishServo(int pitch_angle, int yaw_angle)
{
  std_msgs::UInt16MultiArray array;
  array.data.push_back(pitch_angle);
  array.data.push_back(yaw_angle);
  pitch_ = pitch_angle;
  yaw_ = yaw_angle;
  trackPubServo_.publish(array);
}

void servoCallback(const std_msgs::UInt16MultiArrayConstPtr &msg)
{
  // This callback should always active
  pitch_ = msg->data[0];
  yaw_ = msg->data[1];
}

void resultCallback(const drv_msgs::recognized_targetConstPtr &msg)
{
  tgt_label_ = msg->label;
  int min_x = msg->tgt_bbox_array.data[0];
  int min_y = msg->tgt_bbox_array.data[1];
  int max_x = msg->tgt_bbox_array.data[2];
  int max_y = msg->tgt_bbox_array.data[3];

  roi_init_ = Rect(min_x, min_y, max_x - min_x, max_y - min_y);
  tracker.initialized_ = false;
}

bool verifyDetection(Rect roi) {
  if (roi.area() < 20) {
    ROS_WARN("Target area in image is %d, too small to be tracked.\n", roi.area());
    return false;
  }
  if (roi.x < 0 || roi.x >= 640) {
    ROS_WARN("ROI X is %d.\n", roi.x);
    return false;
  }
  if (roi.y < 0 || roi.y >= 480) {
    ROS_WARN("ROI Y is %d.\n", roi.y);
    return false;
  }
  if (roi.x + roi.width >= 640) {
    ROS_WARN("ROI X+W is %d.\n", roi.x + roi.width);
    return false;
  }
  if (roi.y + roi.height >= 480) {
    ROS_WARN("ROI Y+H is %d.\n", roi.y + roi.height);
    return false;
  }
  if (roi.width <= 0 || roi.height <= 0) {
    ROS_WARN("ROI W, H is %d, %d.\n", roi.width, roi.height);
    return false;
  }
  return true;
}

void pubTarget(std_msgs::Header header, vector<unsigned int> mask_id, Rect roi) {
  // publish new target info
  drv_msgs::recognized_target result;
  result.header = header;
  result.label = tgt_label_;
  result.tgt_pixels.data = mask_id; // the datatype is uint 32 aka unsigned int
  result.tgt_bbox_array.data.push_back(roi.x);
  result.tgt_bbox_array.data.push_back(roi.y);
  result.tgt_bbox_array.data.push_back(roi.x + roi.width);
  result.tgt_bbox_array.data.push_back(roi.y + roi.height);
  result.tgt_bbox_center.data.push_back(roi.x + roi.width / 2);
  result.tgt_bbox_center.data.push_back(roi.y + roi.height / 2);

  trackPubTargetLocation_.publish(result);
}

bool postProcess(Rect roi, Mat &track_img)
{
  if (!verifyDetection(roi))
    return false;

  if (roi.area() < MIN_OBJECT_AREA || roi.area() > MAX_OBJECT_AREA)
    return false;

  Utilities::markImage(roi, track_img);
  return true;
}

void imageCallback(const sensor_msgs::ImageConstPtr& image_msg)
{
  if (modeType_ != m_track)
    return;

  if (!verifyDetection(roi_init_)) {
    isInTracking_ = false;
    tracker.initialized_ = false;
    return;
  }

  src_ = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8);
  src_img_ = src_->image;
  src_img_.copyTo(track_img_);

  // cv::rectangle(track_image_, detection_, cv::Scalar(232,228,53), 2);
  // Adjust the camera view based on ROI center
  if (!tracker.initialized_) {
    tracker.init(roi_init_, src_img_);
    tracker.initialized_ = true;
  }
  else {
    Rect roi = tracker.update(src_img_);

    vector<unsigned int> mask_id; // store object pixels id in image
    if (!postProcess(roi, track_img_)) {
      delay_--;
      if (delay_ < 0) {
        isInTracking_ = false;
        tracker.initialized_ = false;
      }
      return;
    }

    // publish image of target with bounding box
    track_ptr_->header = image_msg->header;
    track_ptr_->image = track_img_;
    track_ptr_->encoding = sensor_msgs::image_encodings::BGR8;
    trackPubImage_.publish(track_ptr_->toImageMsg());

    // drive the camera so that the center of the image captured is on object center
    int d_x = roi.x + roi.width / 2 - 320;
    int d_y = roi.y + roi.height / 2 - 240;
    int deg_x = int(d_x * x_to_angle); // offset the robot head
    int deg_y = int(d_y * y_to_angle);

    isInTracking_ = true;
    delay_ = WAIT_LOOP;
    pubTarget(image_msg->header, mask_id, roi);

    if (abs(deg_x) < angle_step && abs(deg_y) < angle_step) {
      // target on image center, continuing tracking..
      isInTracking_ = true;
      delay_ = WAIT_LOOP;
      pubTarget(image_msg->header, mask_id, roi);
      return;
    }
    if (abs(deg_x) >= angle_step && abs(deg_y) < angle_step) {
      // need move in x direction
      if (deg_x < -angle_step) deg_x = -angle_step;
      if (deg_x > angle_step) deg_x = angle_step;
      deg_y = 0;
    }
    if (abs(deg_x) < angle_step && abs(deg_y) >= angle_step) {
      // need move in y direction
      if (deg_y < -angle_step) deg_y = -angle_step;
      if (deg_y > angle_step) deg_y = angle_step;
      deg_x = 0;
    }
    if (abs(deg_x) >= angle_step && abs(deg_y) >= angle_step) {
      // need move in both directions
      if (deg_x < -angle_step) deg_x = -angle_step;
      if (deg_x > angle_step) deg_x = angle_step;
      if (deg_y < -angle_step) deg_y = -angle_step;
      if (deg_y > angle_step) deg_y = angle_step;
    }
    int x_ang = - deg_x + yaw_;
    int y_ang = - deg_y + pitch_;

    if (!(x_ang >= 0 && x_ang <= 180 && y_ang >= 60 && y_ang <= 140)) {
      // target center out of camera movable region
      pubTarget(image_msg->header, mask_id, roi);
      ROS_WARN_THROTTLE(31, "Target out of camera movable area.");
      // Although out of movable area, still in tracking
      // isInTracking_ = false;
      // tracker.initialized_ = false;
    }
    else {
      // target center is in camera movable region, so move the camera
      publishServo(y_ang, x_ang);
    }
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "drv_track_kcf");

  ros::NodeHandle nh;
  ros::NodeHandle pnh;
  ros::NodeHandle rgb_nh(nh, "rgb");
  ros::NodeHandle rgb_pnh(pnh, "rgb");

  image_transport::ImageTransport it_rgb_sub(rgb_nh);
  image_transport::TransportHints hints_rgb("compressed", ros::TransportHints(), rgb_pnh);

  image_transport::ImageTransport it_rgb_pub(nh);
  trackPubImage_ = it_rgb_pub.advertise("search/labeled_image", 1);
  trackPubServo_ = nh.advertise<std_msgs::UInt16MultiArray>("servo", 1);
  trackPubStatus_ = nh.advertise<std_msgs::Bool>("status/track/feedback", 1);
  trackPubTargetLocation_ = nh.advertise<drv_msgs::recognized_target>("track/recognized_target" , 1);

  ros::Subscriber sub_res = nh.subscribe<drv_msgs::recognized_target>("search/recognized_target", 1, resultCallback);
  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("image_rect_color", 1, imageCallback, hints_rgb);
  ros::Subscriber sub_s = nh.subscribe<std_msgs::UInt16MultiArray>("servo", 1, servoCallback);

  if (ros::param::has(param_servo_pitch))
    ros::param::get(param_servo_pitch, pitch_);

  if (ros::param::has(param_servo_yaw))
    ros::param::get(param_servo_yaw, yaw_);

  ROS_INFO("KCF tracking function initialized.");

  while (ros::ok())
  {
    if (ros::param::has(param_running_mode)) {
      ros::param::get(param_running_mode, modeTypeTemp_);

      if (modeTypeTemp_ != m_track && modeType_ == m_track)
        tracker.initialized_ = false;

      modeType_ = modeTypeTemp_;
    }

    std_msgs::Bool flag;
    flag.data = true;

    ros::spinOnce();

    flag.data = isInTracking_;
    trackPubStatus_.publish(flag);
  }

  return 0;
}
<|endoftext|>


<commit_before>#include <ros/ros.h>

//ROS libraries
#include <angles/angles.h>
#include <random_numbers/random_numbers.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/Int16.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Range.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <apriltags_ros/AprilTagDetectionArray.h>

// To handle shutdown signals so the node quits properly in response to "rosnode kill"
#include <ros/ros.h>
#include <signal.h>

using namespace std;

//Random number generator
random_numbers::RandomNumberGenerator* rng;

//Mobility Logic Functions
void setVelocity(double linearVel, double angularVel);
void openFingers(); // Open fingers to 90 degrees
void closeFingers();// Close fingers to 0 degrees
void raiseWrist();  // Return wrist back to 0 degrees
void lowerWrist();  // Lower wrist to 50 degrees

//Numeric Variables
geometry_msgs::Pose2D currentLocation;
geometry_msgs::Pose2D goalLocation;
int currentMode = 0;
float mobilityLoopTimeStep = 0.1; //time between the mobility loop calls
float status_publish_interval = 5;
float killSwitchTimeout = 10;
std_msgs::Int16 targetDetected; //ID of the detected target
bool targetsCollected [256] = {0}; //array of booleans indicating whether each target ID has been found

// state machine states
#define STATE_MACHINE_TRANSFORM	0
#define STATE_MACHINE_ROTATE	1
#define STATE_MACHINE_TRANSLATE	2
int stateMachineState = STATE_MACHINE_TRANSFORM;

geometry_msgs::Twist velocity;
char host[128];
string publishedName;
char prev_state_machine[128];

//Publishers
ros::Publisher velocityPublish;
ros::Publisher stateMachinePublish;
ros::Publisher status_publisher;
ros::Publisher targetCollectedPublish;
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher infoLogPublisher;

//Subscribers
ros::Subscriber joySubscriber;
ros::Subscriber modeSubscriber;
ros::Subscriber targetSubscriber;
ros::Subscriber obstacleSubscriber;
ros::Subscriber odometrySubscriber;
ros::Subscriber targetsCollectedSubscriber;

//Timers
ros::Timer stateMachineTimer;
ros::Timer publish_status_timer;
ros::Timer killSwitchTimer;

// OS Signal Handler
void sigintEventHandler(int signal);

//Callback handlers
void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message);
void modeHandler(const std_msgs::UInt8::ConstPtr& message);
void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tagInfo);
void obstacleHandler(const std_msgs::UInt8::ConstPtr& message);
void odometryHandler(const nav_msgs::Odometry::ConstPtr& message);
void mobilityStateMachine(const ros::TimerEvent&);
void publishStatusTimerEventHandler(const ros::TimerEvent& event);
void targetsCollectedHandler(const std_msgs::Int16::ConstPtr& message);
void killSwitchTimerEventHandler(const ros::TimerEvent& event);

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);

    rng = new random_numbers::RandomNumberGenerator(); //instantiate random number generator
    goalLocation.theta = rng->uniformReal(0, 2 * M_PI); //set initial random heading

    targetDetected.data = -1; //initialize target detected

    //select initial search position 50 cm from center (0,0)
	goalLocation.x = 0.5 * cos(goalLocation.theta);
	goalLocation.y = 0.5 * sin(goalLocation.theta);

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  Mobility module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    // NoSignalHandler so we can catch SIGINT ourselves and shutdown the node
    ros::init(argc, argv, (publishedName + "_MOBILITY"), ros::init_options::NoSigintHandler);
    ros::NodeHandle mNH;

    signal(SIGINT, sigintEventHandler); // Register the SIGINT event handler so the node can shutdown properly

    joySubscriber = mNH.subscribe((publishedName + "/joystick"), 10, joyCmdHandler);
    modeSubscriber = mNH.subscribe((publishedName + "/mode"), 1, modeHandler);
    targetSubscriber = mNH.subscribe((publishedName + "/targets"), 10, targetHandler);
    obstacleSubscriber = mNH.subscribe((publishedName + "/obstacle"), 10, obstacleHandler);
    odometrySubscriber = mNH.subscribe((publishedName + "/odom/filtered"), 10, odometryHandler);
    targetsCollectedSubscriber = mNH.subscribe(("targetsCollected"), 10, targetsCollectedHandler);

    status_publisher = mNH.advertise<std_msgs::String>((publishedName + "/status"), 1, true);
    velocityPublish = mNH.advertise<geometry_msgs::Twist>((publishedName + "/velocity"), 10);
    stateMachinePublish = mNH.advertise<std_msgs::String>((publishedName + "/state_machine"), 1, true);
    targetCollectedPublish = mNH.advertise<std_msgs::Int16>(("targetsCollected"), 1, true);
    fingerAnglePublish = mNH.advertise<std_msgs::Float32>((publishedName + "/fingerAngle"), 1, true);
    wristAnglePublish = mNH.advertise<std_msgs::Float32>((publishedName + "/wristAngle"), 1, true);
    infoLogPublisher = mNH.advertise<std_msgs::String>("/infoLog", 1, true);

    publish_status_timer = mNH.createTimer(ros::Duration(status_publish_interval), publishStatusTimerEventHandler);
    //killSwitchTimer = mNH.createTimer(ros::Duration(killSwitchTimeout), killSwitchTimerEventHandler);
    stateMachineTimer = mNH.createTimer(ros::Duration(mobilityLoopTimeStep), mobilityStateMachine);

    std_msgs::String msg;
    msg.data = "Log Started";
    infoLogPublisher.publish(msg);
    ros::spin();

    return EXIT_SUCCESS;
}

void mobilityStateMachine(const ros::TimerEvent&) {
    std_msgs::String stateMachineMsg;

    if (currentMode == 2 || currentMode == 3) { //Robot is in automode

		switch(stateMachineState) {

			//Select rotation or translation based on required adjustment
			//If no adjustment needed, select new goal
			case STATE_MACHINE_TRANSFORM: {
				stateMachineMsg.data = "TRANSFORMING";
				//If angle between current and goal is significant
				if (fabs(angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta)) > 0.1) {
					stateMachineState = STATE_MACHINE_ROTATE; //rotate
				}
				//If goal has not yet been reached
				else if (fabs(angles::shortest_angular_distance(currentLocation.theta, atan2(goalLocation.y - currentLocation.y, goalLocation.x - currentLocation.x))) < M_PI_2) {
					stateMachineState = STATE_MACHINE_TRANSLATE; //translate
				}
				//If returning with a target
				else if (targetDetected.data != -1) {
					//If goal has not yet been reached
					if (hypot(0.0 - currentLocation.x, 0.0 - currentLocation.y) > 0.5) {
				        //set angle to center as goal heading
						goalLocation.theta = M_PI + atan2(currentLocation.y, currentLocation.x);

						//set center as goal position
						goalLocation.x = 0.0;
						goalLocation.y = 0.0;
					}
					//Otherwise, reset target and select new random uniform heading
					else {
						targetDetected.data = -1;
						goalLocation.theta = rng->uniformReal(0, 2 * M_PI);
					}
				}
				//Otherwise, assign a new goal
				else {
					 //select new heading from Gaussian distribution around current heading
					goalLocation.theta = rng->gaussian(currentLocation.theta, 0.25);

					//select new position 50 cm from current location
					goalLocation.x = currentLocation.x + (0.5 * cos(goalLocation.theta));
					goalLocation.y = currentLocation.y + (0.5 * sin(goalLocation.theta));
				}

				//Purposefully fall through to next case without breaking
			}

			//Calculate angle between currentLocation.theta and goalLocation.theta
			//Rotate left or right depending on sign of angle
			//Stay in this state until angle is minimized
			case STATE_MACHINE_ROTATE: {
				stateMachineMsg.data = "ROTATING";
			    if (angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta) > 0.1) {
					setVelocity(0.0, 0.2); //rotate left
			    }
			    else if (angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta) < -0.1) {
					setVelocity(0.0, -0.2); //rotate right
				}
				else {
					setVelocity(0.0, 0.0); //stop
					stateMachineState = STATE_MACHINE_TRANSLATE; //move to translate step
				}
			    break;
			}

			//Calculate angle between currentLocation.x/y and goalLocation.x/y
			//Drive forward
			//Stay in this state until angle is at least PI/2
			case STATE_MACHINE_TRANSLATE: {
				stateMachineMsg.data = "TRANSLATING";
				if (fabs(angles::shortest_angular_distance(currentLocation.theta, atan2(goalLocation.y - currentLocation.y, goalLocation.x - currentLocation.x))) < M_PI_2) {
					setVelocity(0.3, 0.0);
				}
				else {
					setVelocity(0.0, 0.0); //stop
					stateMachineState = STATE_MACHINE_TRANSFORM; //move back to transform step
				}
			    break;
			}

			default: {
			    break;
			}
		}
	}

    else { // mode is NOT auto

        // publish current state for the operator to see
        stateMachineMsg.data = "WAITING";
    }

    // publish state machine string for user, only if it has changed, though
    if (strcmp(stateMachineMsg.data.c_str(), prev_state_machine) != 0) {
        stateMachinePublish.publish(stateMachineMsg);
        sprintf(prev_state_machine, "%s", stateMachineMsg.data.c_str());
    }
}

void setVelocity(double linearVel, double angularVel)
{
  // Stopping and starting the timer causes it to start counting from 0 again.
  // As long as this is called before the kill swith timer reaches killSwitchTimeout seconds
  // the rover's kill switch wont be called.
  killSwitchTimer.stop();
  killSwitchTimer.start();

  velocity.linear.x = linearVel * 1.5;
  velocity.angular.z = angularVel * 8; //scaling factor for sim; removed by aBridge node
  velocityPublish.publish(velocity);
}

/***********************
 * ROS CALLBACK HANDLERS
 ************************/

void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& message) {

	if (message->detections.size() > 0) {
		//if this is the goal target
		if (message->detections[0].id == 256) {
			//if we were returning with a target
		    if (targetDetected.data != -1) {
				targetDetected.data = -1;
		    }
		}

		//if target has not previously been detected
		else if (targetDetected.data == -1) {

	        //check if target has not yet been collected
	        if (!targetsCollected[message->detections[0].id]) {
				//copy target ID to class variable
				targetDetected.data = message->detections[0].id;

		        //set angle to center as goal heading
				goalLocation.theta = M_PI + atan2(currentLocation.y, currentLocation.x);

				//set center as goal position
				goalLocation.x = 0.0;
				goalLocation.y = 0.0;

				//publish detected target
				targetCollectedPublish.publish(targetDetected);

				//switch to transform state to trigger return to center
				stateMachineState = STATE_MACHINE_TRANSFORM;
			}
	    }
	}
}

void modeHandler(const std_msgs::UInt8::ConstPtr& message) {
	currentMode = message->data;
	setVelocity(0.0, 0.0);
}

void obstacleHandler(const std_msgs::UInt8::ConstPtr& message) {
	if (message->data > 0) {
		//obstacle on right side
		if (message->data == 1) {
			//select new heading 0.2 radians to the left
			goalLocation.theta = currentLocation.theta + 0.2;
		}

		//obstacle in front or on left side
		else if (message->data == 2) {
			//select new heading 0.2 radians to the right
			goalLocation.theta = currentLocation.theta - 0.2;
		}

		//select new position 50 cm from current location
		goalLocation.x = currentLocation.x + (0.5 * cos(goalLocation.theta));
		goalLocation.y = currentLocation.y + (0.5 * sin(goalLocation.theta));

		//switch to transform state to trigger collision avoidance
		stateMachineState = STATE_MACHINE_TRANSFORM;
	}
}

void odometryHandler(const nav_msgs::Odometry::ConstPtr& message) {
	//Get (x,y) location directly from pose
	currentLocation.x = message->pose.pose.position.x;
	currentLocation.y = message->pose.pose.position.y;

	//Get theta rotation by converting quaternion orientation to pitch/roll/yaw
	tf::Quaternion q(message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z, message->pose.pose.orientation.w);
	tf::Matrix3x3 m(q);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);
	currentLocation.theta = yaw;
}

void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message) {
	if (currentMode == 0 || currentMode == 1) {
		setVelocity(abs(message->axes[4]) >= 0.1 ? message->axes[4] : 0, abs(message->axes[3]) >= 0.1 ? message->axes[3] : 0);
	}
}

void publishStatusTimerEventHandler(const ros::TimerEvent&)
{
  std_msgs::String msg;
  msg.data = "online";
  status_publisher.publish(msg);
}

// Safety precaution. No movement commands - might have lost contact with ROS. Stop the rover.
// Also might no longer be receiving manual movement commands so stop the rover.
void killSwitchTimerEventHandler(const ros::TimerEvent& t)
{
  // No movement commands for killSwitchTime seconds so stop the rover
  setVelocity(0,0);
  double current_time = ros::Time::now().toSec();
  ROS_INFO("In mobility.cpp:: killSwitchTimerEventHander(): Movement input timeout. Stopping the rover at %6.4f.", current_time);
}

void targetsCollectedHandler(const std_msgs::Int16::ConstPtr& message) {
	targetsCollected[message->data] = 1;
}

void sigintEventHandler(int sig)
{
     // All the default sigint handler does is call shutdown()
     ros::shutdown();
}

void openFingers()
{
    // Opens fingers/claw to 50 degrees
    std_msgs::Float32 msg;
    msg.data = 90;
    fingerAnglePublish.publish(msg);
}

void closeFingers()
{
    // Close fingers to 0 degrees
    std_msgs::Float32 msg;
    msg.data = 0;
    fingerAnglePublish.publish(msg);
}

void raiseWrist()
{
    // Return wrist back to neutral position at 0 degrees
    std_msgs::Float32 msg;
    msg.data = 0;
    wristAnglePublish.publish(msg);
}

void lowerWrist()
{
    // Lowers wrist to just above the ground at 50 degrees
    std_msgs::Float32 msg;
    msg.data = 50;
    wristAnglePublish.publish(msg);
}

<commit_msg>Remove finger/wrist angle helper functions from mobility<commit_after>#include <ros/ros.h>

//ROS libraries
#include <angles/angles.h>
#include <random_numbers/random_numbers.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/Int16.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Range.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <apriltags_ros/AprilTagDetectionArray.h>

// To handle shutdown signals so the node quits properly in response to "rosnode kill"
#include <ros/ros.h>
#include <signal.h>

using namespace std;

//Random number generator
random_numbers::RandomNumberGenerator* rng;

//Mobility Logic Functions
void setVelocity(double linearVel, double angularVel);
void openFingers(); // Open fingers to 90 degrees
void closeFingers();// Close fingers to 0 degrees
void raiseWrist();  // Return wrist back to 0 degrees
void lowerWrist();  // Lower wrist to 50 degrees

//Numeric Variables
geometry_msgs::Pose2D currentLocation;
geometry_msgs::Pose2D goalLocation;
int currentMode = 0;
float mobilityLoopTimeStep = 0.1; //time between the mobility loop calls
float status_publish_interval = 5;
float killSwitchTimeout = 10;
std_msgs::Int16 targetDetected; //ID of the detected target
bool targetsCollected [256] = {0}; //array of booleans indicating whether each target ID has been found

// state machine states
#define STATE_MACHINE_TRANSFORM	0
#define STATE_MACHINE_ROTATE	1
#define STATE_MACHINE_TRANSLATE	2
int stateMachineState = STATE_MACHINE_TRANSFORM;

geometry_msgs::Twist velocity;
char host[128];
string publishedName;
char prev_state_machine[128];

//Publishers
ros::Publisher velocityPublish;
ros::Publisher stateMachinePublish;
ros::Publisher status_publisher;
ros::Publisher targetCollectedPublish;
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher infoLogPublisher;

//Subscribers
ros::Subscriber joySubscriber;
ros::Subscriber modeSubscriber;
ros::Subscriber targetSubscriber;
ros::Subscriber obstacleSubscriber;
ros::Subscriber odometrySubscriber;
ros::Subscriber targetsCollectedSubscriber;

//Timers
ros::Timer stateMachineTimer;
ros::Timer publish_status_timer;
ros::Timer killSwitchTimer;

// OS Signal Handler
void sigintEventHandler(int signal);

//Callback handlers
void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message);
void modeHandler(const std_msgs::UInt8::ConstPtr& message);
void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& tagInfo);
void obstacleHandler(const std_msgs::UInt8::ConstPtr& message);
void odometryHandler(const nav_msgs::Odometry::ConstPtr& message);
void mobilityStateMachine(const ros::TimerEvent&);
void publishStatusTimerEventHandler(const ros::TimerEvent& event);
void targetsCollectedHandler(const std_msgs::Int16::ConstPtr& message);
void killSwitchTimerEventHandler(const ros::TimerEvent& event);

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);

    rng = new random_numbers::RandomNumberGenerator(); //instantiate random number generator
    goalLocation.theta = rng->uniformReal(0, 2 * M_PI); //set initial random heading

    targetDetected.data = -1; //initialize target detected

    //select initial search position 50 cm from center (0,0)
	goalLocation.x = 0.5 * cos(goalLocation.theta);
	goalLocation.y = 0.5 * sin(goalLocation.theta);

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  Mobility module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    // NoSignalHandler so we can catch SIGINT ourselves and shutdown the node
    ros::init(argc, argv, (publishedName + "_MOBILITY"), ros::init_options::NoSigintHandler);
    ros::NodeHandle mNH;

    signal(SIGINT, sigintEventHandler); // Register the SIGINT event handler so the node can shutdown properly

    joySubscriber = mNH.subscribe((publishedName + "/joystick"), 10, joyCmdHandler);
    modeSubscriber = mNH.subscribe((publishedName + "/mode"), 1, modeHandler);
    targetSubscriber = mNH.subscribe((publishedName + "/targets"), 10, targetHandler);
    obstacleSubscriber = mNH.subscribe((publishedName + "/obstacle"), 10, obstacleHandler);
    odometrySubscriber = mNH.subscribe((publishedName + "/odom/filtered"), 10, odometryHandler);
    targetsCollectedSubscriber = mNH.subscribe(("targetsCollected"), 10, targetsCollectedHandler);

    status_publisher = mNH.advertise<std_msgs::String>((publishedName + "/status"), 1, true);
    velocityPublish = mNH.advertise<geometry_msgs::Twist>((publishedName + "/velocity"), 10);
    stateMachinePublish = mNH.advertise<std_msgs::String>((publishedName + "/state_machine"), 1, true);
    targetCollectedPublish = mNH.advertise<std_msgs::Int16>(("targetsCollected"), 1, true);
    fingerAnglePublish = mNH.advertise<std_msgs::Float32>((publishedName + "/fingerAngle"), 1, true);
    wristAnglePublish = mNH.advertise<std_msgs::Float32>((publishedName + "/wristAngle"), 1, true);
    infoLogPublisher = mNH.advertise<std_msgs::String>("/infoLog", 1, true);

    publish_status_timer = mNH.createTimer(ros::Duration(status_publish_interval), publishStatusTimerEventHandler);
    //killSwitchTimer = mNH.createTimer(ros::Duration(killSwitchTimeout), killSwitchTimerEventHandler);
    stateMachineTimer = mNH.createTimer(ros::Duration(mobilityLoopTimeStep), mobilityStateMachine);

    std_msgs::String msg;
    msg.data = "Log Started";
    infoLogPublisher.publish(msg);
    ros::spin();

    return EXIT_SUCCESS;
}

void mobilityStateMachine(const ros::TimerEvent&) {
    std_msgs::String stateMachineMsg;

    if (currentMode == 2 || currentMode == 3) { //Robot is in automode

		switch(stateMachineState) {

			//Select rotation or translation based on required adjustment
			//If no adjustment needed, select new goal
			case STATE_MACHINE_TRANSFORM: {
				stateMachineMsg.data = "TRANSFORMING";
				//If angle between current and goal is significant
				if (fabs(angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta)) > 0.1) {
					stateMachineState = STATE_MACHINE_ROTATE; //rotate
				}
				//If goal has not yet been reached
				else if (fabs(angles::shortest_angular_distance(currentLocation.theta, atan2(goalLocation.y - currentLocation.y, goalLocation.x - currentLocation.x))) < M_PI_2) {
					stateMachineState = STATE_MACHINE_TRANSLATE; //translate
				}
				//If returning with a target
				else if (targetDetected.data != -1) {
					//If goal has not yet been reached
					if (hypot(0.0 - currentLocation.x, 0.0 - currentLocation.y) > 0.5) {
				        //set angle to center as goal heading
						goalLocation.theta = M_PI + atan2(currentLocation.y, currentLocation.x);

						//set center as goal position
						goalLocation.x = 0.0;
						goalLocation.y = 0.0;
					}
					//Otherwise, reset target and select new random uniform heading
					else {
						targetDetected.data = -1;
						goalLocation.theta = rng->uniformReal(0, 2 * M_PI);
					}
				}
				//Otherwise, assign a new goal
				else {
					 //select new heading from Gaussian distribution around current heading
					goalLocation.theta = rng->gaussian(currentLocation.theta, 0.25);

					//select new position 50 cm from current location
					goalLocation.x = currentLocation.x + (0.5 * cos(goalLocation.theta));
					goalLocation.y = currentLocation.y + (0.5 * sin(goalLocation.theta));
				}

				//Purposefully fall through to next case without breaking
			}

			//Calculate angle between currentLocation.theta and goalLocation.theta
			//Rotate left or right depending on sign of angle
			//Stay in this state until angle is minimized
			case STATE_MACHINE_ROTATE: {
				stateMachineMsg.data = "ROTATING";
			    if (angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta) > 0.1) {
					setVelocity(0.0, 0.2); //rotate left
			    }
			    else if (angles::shortest_angular_distance(currentLocation.theta, goalLocation.theta) < -0.1) {
					setVelocity(0.0, -0.2); //rotate right
				}
				else {
					setVelocity(0.0, 0.0); //stop
					stateMachineState = STATE_MACHINE_TRANSLATE; //move to translate step
				}
			    break;
			}

			//Calculate angle between currentLocation.x/y and goalLocation.x/y
			//Drive forward
			//Stay in this state until angle is at least PI/2
			case STATE_MACHINE_TRANSLATE: {
				stateMachineMsg.data = "TRANSLATING";
				if (fabs(angles::shortest_angular_distance(currentLocation.theta, atan2(goalLocation.y - currentLocation.y, goalLocation.x - currentLocation.x))) < M_PI_2) {
					setVelocity(0.3, 0.0);
				}
				else {
					setVelocity(0.0, 0.0); //stop
					stateMachineState = STATE_MACHINE_TRANSFORM; //move back to transform step
				}
			    break;
			}

			default: {
			    break;
			}
		}
	}

    else { // mode is NOT auto

        // publish current state for the operator to see
        stateMachineMsg.data = "WAITING";
    }

    // publish state machine string for user, only if it has changed, though
    if (strcmp(stateMachineMsg.data.c_str(), prev_state_machine) != 0) {
        stateMachinePublish.publish(stateMachineMsg);
        sprintf(prev_state_machine, "%s", stateMachineMsg.data.c_str());
    }
}

void setVelocity(double linearVel, double angularVel)
{
  // Stopping and starting the timer causes it to start counting from 0 again.
  // As long as this is called before the kill swith timer reaches killSwitchTimeout seconds
  // the rover's kill switch wont be called.
  killSwitchTimer.stop();
  killSwitchTimer.start();

  velocity.linear.x = linearVel * 1.5;
  velocity.angular.z = angularVel * 8; //scaling factor for sim; removed by aBridge node
  velocityPublish.publish(velocity);
}

/***********************
 * ROS CALLBACK HANDLERS
 ************************/

void targetHandler(const apriltags_ros::AprilTagDetectionArray::ConstPtr& message) {

	if (message->detections.size() > 0) {
		//if this is the goal target
		if (message->detections[0].id == 256) {
			//if we were returning with a target
		    if (targetDetected.data != -1) {
				targetDetected.data = -1;
		    }
		}

		//if target has not previously been detected
		else if (targetDetected.data == -1) {

	        //check if target has not yet been collected
	        if (!targetsCollected[message->detections[0].id]) {
				//copy target ID to class variable
				targetDetected.data = message->detections[0].id;

		        //set angle to center as goal heading
				goalLocation.theta = M_PI + atan2(currentLocation.y, currentLocation.x);

				//set center as goal position
				goalLocation.x = 0.0;
				goalLocation.y = 0.0;

				//publish detected target
				targetCollectedPublish.publish(targetDetected);

				//switch to transform state to trigger return to center
				stateMachineState = STATE_MACHINE_TRANSFORM;
			}
	    }
	}
}

void modeHandler(const std_msgs::UInt8::ConstPtr& message) {
	currentMode = message->data;
	setVelocity(0.0, 0.0);
}

void obstacleHandler(const std_msgs::UInt8::ConstPtr& message) {
	if (message->data > 0) {
		//obstacle on right side
		if (message->data == 1) {
			//select new heading 0.2 radians to the left
			goalLocation.theta = currentLocation.theta + 0.2;
		}

		//obstacle in front or on left side
		else if (message->data == 2) {
			//select new heading 0.2 radians to the right
			goalLocation.theta = currentLocation.theta - 0.2;
		}

		//select new position 50 cm from current location
		goalLocation.x = currentLocation.x + (0.5 * cos(goalLocation.theta));
		goalLocation.y = currentLocation.y + (0.5 * sin(goalLocation.theta));

		//switch to transform state to trigger collision avoidance
		stateMachineState = STATE_MACHINE_TRANSFORM;
	}
}

void odometryHandler(const nav_msgs::Odometry::ConstPtr& message) {
	//Get (x,y) location directly from pose
	currentLocation.x = message->pose.pose.position.x;
	currentLocation.y = message->pose.pose.position.y;

	//Get theta rotation by converting quaternion orientation to pitch/roll/yaw
	tf::Quaternion q(message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z, message->pose.pose.orientation.w);
	tf::Matrix3x3 m(q);
	double roll, pitch, yaw;
	m.getRPY(roll, pitch, yaw);
	currentLocation.theta = yaw;
}

void joyCmdHandler(const sensor_msgs::Joy::ConstPtr& message) {
	if (currentMode == 0 || currentMode == 1) {
		setVelocity(abs(message->axes[4]) >= 0.1 ? message->axes[4] : 0, abs(message->axes[3]) >= 0.1 ? message->axes[3] : 0);
	}
}

void publishStatusTimerEventHandler(const ros::TimerEvent&)
{
  std_msgs::String msg;
  msg.data = "online";
  status_publisher.publish(msg);
}

// Safety precaution. No movement commands - might have lost contact with ROS. Stop the rover.
// Also might no longer be receiving manual movement commands so stop the rover.
void killSwitchTimerEventHandler(const ros::TimerEvent& t)
{
  // No movement commands for killSwitchTime seconds so stop the rover
  setVelocity(0,0);
  double current_time = ros::Time::now().toSec();
  ROS_INFO("In mobility.cpp:: killSwitchTimerEventHander(): Movement input timeout. Stopping the rover at %6.4f.", current_time);
}

void targetsCollectedHandler(const std_msgs::Int16::ConstPtr& message) {
	targetsCollected[message->data] = 1;
}

void sigintEventHandler(int sig)
{
     // All the default sigint handler does is call shutdown()
     ros::shutdown();
}
<|endoftext|>


<commit_before>#include "stdafx.h"
#include "CppUnitTest.h"
#include "..\..\uti.hpp"

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

template __declspec( dllexport ) class uti::UTF8String<>;

typedef uti::UTF8String<> String;

namespace utiTest
{

}<commit_msg>Added UTF16String to second unit test<commit_after>#include "stdafx.h"
#include "CppUnitTest.h"
#include "..\..\uti.hpp"

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

template __declspec( dllexport ) class uti::UTF8String<>;

template __declspec( dllexport ) class uti::UTF16String<>;

typedef uti::UTF8String<> String;

namespace utiTest
{

}<|endoftext|>


<commit_before>#include <iostream>
#include "queue_by_2stacks.hpp"
#include "queue.hpp"
#include <vector>
#include "stack_by_2queues.hpp"

int main()
{
    ch10::stack_by_2queues<int> stk(99);

    return 0;
}

<commit_msg>	modified:   main.cpp<commit_after>#include <iostream>
#include "stack_by_2queues.hpp"

int main()
{
    ch10::stack_by_2queues<int> stk(3);

    for(int i = 0; i != 3; ++i)
        stk.push(i);

    for(int i = 0; i != 3; ++i)
        std::cout << stk.pop() << std::endl;

    return 0;
}

<|endoftext|>


<commit_before>#include <osg/GraphicsContext>
#include <osgDB/ReadFile>
#include <osgViewer/Viewer>

int main(
    int /*argc*/,
    char** /*argv*/)
{
    osg::ref_ptr<osg::GraphicsContext::Traits> traits =
        new osg::GraphicsContext::Traits;
    traits->x = 50;
    traits->y = 50;
    traits->width = 800;
    traits->height = 600;
    traits->windowDecoration = true;
    traits->doubleBuffer = true;
    traits->samples = 4;

    osg::ref_ptr<osg::GraphicsContext> gc =
        osg::GraphicsContext::createGraphicsContext(traits.get());
    osg::ref_ptr<osg::Camera> camera = new osg::Camera;
    camera->setGraphicsContext(gc.get());
    camera->setViewport(0, 0, traits->width, traits->height);
    camera->setClearMask(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    camera->setClearColor(osg::Vec4(0.2f, 0.2f, 0.4f, 1.0f));
    camera->setProjectionMatrixAsPerspective(
        30.0f,
        static_cast<double>(traits->width) / traits->height,
        1.0,
        1000.0);

    osg::ref_ptr<osg::Node> root = osgDB::readNodeFile("cessna.osg");
    osgViewer::Viewer viewer;
    viewer.setCamera(camera.get());
    viewer.setSceneData(root.get());
    return viewer.run();
}
<commit_msg>Fix chapter 9 example 4<commit_after>#include <osg/GraphicsContext>
#include <osgDB/ReadFile>
#include <osgViewer/Viewer>

int main(
    int /*argc*/,
    char** /*argv*/)
{
    osg::ref_ptr<osg::GraphicsContext::Traits> traits =
        new osg::GraphicsContext::Traits;
    traits->x = 50;
    traits->y = 50;
    traits->width = 800;
    traits->height = 600;
    traits->windowDecoration = true;
    traits->doubleBuffer = true;
    traits->samples = 4;

    osg::ref_ptr<osg::GraphicsContext> gc =
        osg::GraphicsContext::createGraphicsContext(traits.get());
    osg::ref_ptr<osg::Camera> camera = new osg::Camera;
    camera->setGraphicsContext(gc.get());
    camera->setViewport(0, 0, traits->width, traits->height);
    camera->getOrCreateStateSet()->setMode(GL_DEPTH_TEST, osg::StateAttribute::ON);
    camera->setClearMask(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    camera->setClearColor(osg::Vec4(0.2f, 0.2f, 0.4f, 1.0f));
    camera->setProjectionMatrixAsPerspective(
        30.0f,
        static_cast<double>(traits->width) / traits->height,
        1.0,
        1000.0);

    osg::ref_ptr<osg::Node> root = osgDB::readNodeFile("cessna.osg");
    osgViewer::Viewer viewer;
    viewer.setCamera(camera.get());
    viewer.setSceneData(root.get());
    return viewer.run();
}
<|endoftext|>