<commit_before>#include "Function.h"
#include "../../Units/Units.h"
#include "../../Node/Node.h"
#include "../../ParseResult/ParseResult.h"
#include "../../Function/Function.h"
#include "../../CoreForms/CoreForms.h"
#include "../Linkage/Linkage.h"
#include "../Type/Type.h"
#include "../Function/Function.h"
#include "../ProcBody/ProcBody.h"
#include "../Parameter/Parameter.h"
#include "../Utils/Utils.h"
#include "../../llvm_Function.h"
#include "Config.h"

using namespace dale::ErrorInst;

namespace dale
{
bool
parseFunctionAttributes(Context *ctx, std::vector<Node *> *attr_list,
                        bool *always_inline, bool *cto)
{
    for (std::vector<Node*>::iterator b = (attr_list->begin() + 1),
                                      e = attr_list->end();
            b != e;
            ++b) {
        if ((*b)->is_list) {
            Error *e = new Error(InvalidAttribute, (*b));
            ctx->er->addError(e);
            return false;
        }
        if (!((*b)->token->str_value.compare("inline"))) {
            *always_inline = true;
        } else if (!((*b)->token->str_value.compare("cto"))) {
            *cto = true;
        } else {
            Error *e = new Error(InvalidAttribute, (*b));
            ctx->er->addError(e);
            return false;
        }
    }
    return true;
}

bool
parseParameters(Units *units, Node *args_node,
               std::vector<Variable *> *fn_args_internal)
{
    Context *ctx = units->top()->ctx;
    std::vector<Node *> *args = args_node->list;

    for (std::vector<Node *>::iterator b = args->begin(),
                                       e = args->end();
            b != e;
            ++b) {
        Variable *var = new Variable();
        var->type = NULL;

        FormParameterParse(units, var, (*b), false, false, true);
        if (var->type == NULL) {
            delete var;
            return false;
        }

        if (var->type->is_array) {
            delete var;
            Error *e = new Error(ArraysCannotBeFunctionParameters,
                                 (*b));
            ctx->er->addError(e);
            return false;
        }

        if (var->type->base_type == BaseType::Void) {
            delete var;
            if (args->size() != 1) {
                Error *e = new Error(VoidMustBeTheOnlyParameter,
                                     args_node);
                ctx->er->addError(e);
                return false;
            }
            break;
        }

        if (var->type->base_type == BaseType::VarArgs) {
            if ((args->end() - b) != 1) {
                delete var;
                Error *e = new Error(VarArgsMustBeLastParameter,
                                     args_node);
                ctx->er->addError(e);
                return false;
            }
            fn_args_internal->push_back(var);
            break;
        }

        if (var->type->is_function) {
            delete var;
            Error *e = new Error(NonPointerFunctionParameter, (*b));
            ctx->er->addError(e);
            return false;
        }

        fn_args_internal->push_back(var);
    }

    return true;
}

bool
FormFunctionParse(Units *units, Node *node, const char *name,
                  Function **new_fn, int override_linkage,
                  bool is_anonymous)
{
    Context *ctx = units->top()->ctx;

    if (!name) {
        Node *name_node = (*(node->list))[1];
        name = name_node->token->str_value.c_str();
        node = (*(node->list))[2];
    }

    if (!is_anonymous) {
        units->prefunction_ns = ctx->ns();
    }

    if (CoreForms::existsNoOverride(name)) {
        Error *e = new Error(ThisCoreFormCannotBeOverridden, node);
        ctx->er->addError(e);
        return false;
    }

    std::vector<Node *> *lst = node->list;

    if (lst->size() < 4) {
        Error *e = new Error(IncorrectMinimumNumberOfArgs, node, "fn",
                             "3", (lst->size() - 1));
        ctx->er->addError(e);
        return false;
    }

    int next_index = 1;
    bool always_inline = false;
    bool cto = units->cto;

    /* Whole modules, as well as specific functions, can be declared
     * compile-time-only.  If global CTO is enabled, that overrides
     * the absence of a CTO attribute here.  However, if a function
     * explicitly declares that it is CTO, that will override the
     * global setting. */

    Node *test = ((*lst)[next_index]);
    if (test->is_list
            && (*test->list)[0]->is_token
            && !((*test->list)[0]->token->str_value.compare("attr"))) {
        bool res = parseFunctionAttributes(ctx, test->list,
                                           &always_inline, &cto);
        if (!res) {
            return false;
        }
        ++next_index;
    }

    int linkage =
        (override_linkage)
            ? override_linkage
            : FormLinkageParse(ctx, (*lst)[next_index]);
    if (!linkage) {
        return false;
    }
    if (!override_linkage) {
        ++next_index;
    }

    /* The return type is not parsed yet, because it may depend on the
     * types of the function parameters. */

    int return_type_index = next_index;

    Node *args_node = (*lst)[next_index + 1];
    if (!args_node->is_list) {
        Error *e = new Error(UnexpectedElement, args_node,
                             "list", "parameters", "symbol");
        ctx->er->addError(e);
        return false;
    }

    std::vector<Variable *> fn_args_internal;
    bool res = parseParameters(units, args_node, &fn_args_internal);
    if (!res) {
        return false;
    }
    bool varargs = false;
    if (fn_args_internal.size()
            && (fn_args_internal.back()->type->base_type == BaseType::VarArgs)) {
        varargs = true;
    }

    std::vector<llvm::Type*> fn_args;
    for (std::vector<Variable *>::iterator b = fn_args_internal.begin(),
                                           e = fn_args_internal.end();
            b != e;
            ++b) {
        Type *type = (*b)->type;
        if (type->is_reference) {
            type = ctx->tr->getPointerType(type);
        }
        if (type->base_type == BaseType::VarArgs) {
            break;
        }
        llvm::Type *llvm_type = ctx->toLLVMType(type, NULL, false);
        if (!llvm_type) {
            return false;
        }
        fn_args.push_back(llvm_type);
    }

    /* For return type parsing, activate an anonymous namespace and
     * add all of the function parameters to it.  This is so that if
     * the return type uses a macro that depends on one of those
     * parameters, it will work properly. */

    ctx->activateAnonymousNamespace();
    std::string anon_name = ctx->ns()->name;

    for (std::vector<Variable *>::iterator b = fn_args_internal.begin(),
                                           e = fn_args_internal.end();
            b != e;
            ++b) {
        ctx->ns()->addVariable((*b)->name.c_str(), (*b));
    }

    Type *ret_type = FormTypeParse(units, (*lst)[return_type_index],
                                   false, false, false, true);

    ctx->deactivateNamespace(anon_name.c_str());

    if (ret_type == NULL) {
        return false;
    }
    if (ret_type->is_array) {
        Error *e = new Error(ReturnTypesCannotBeArrays, (*lst)[next_index]);
        ctx->er->addError(e);
        return false;
    }

    llvm::Type *llvm_ret_type = ctx->toLLVMType(ret_type, NULL, true);
    if (!llvm_ret_type) {
        return false;
    }

    /* Create the LLVM function type.  If the retval attribute is
     * present, then the LLVM function type will have a return type of
     * void, and a pointer to a value of the return type will be the
     * final parameter. */

    if (ret_type->is_retval) {
        fn_args.push_back(ctx->toLLVMType(ctx->tr->getPointerType(ret_type),
                                          NULL, true));
        llvm_ret_type = ctx->toLLVMType(ctx->tr->getBasicType(BaseType::Void),
                                        NULL, true);
    }
    llvm::FunctionType *ft = getFunctionType(llvm_ret_type, fn_args,
                                             varargs);

    std::string symbol;
    ctx->ns()->functionNameToSymbol(name, &symbol, linkage,
                                    &fn_args_internal);

    Function *fn = new Function(ret_type, &fn_args_internal, NULL, 0,
                                 &symbol, always_inline);
    fn->linkage = linkage;
    fn->cto = cto;
    if (!strcmp(name, "setf-copy") || !strcmp(name, "setf-assign")) {
        fn->is_setf_fn = true;
    } else if (!strcmp(name, "destroy")) {
        fn->is_destructor = true;
    }

    if (fn->is_setf_fn) {
        if (!ret_type->isEqualTo(ctx->tr->type_bool)) {
            Error *e = new Error(SetfOverridesMustReturnBool,
                                 (*lst)[return_type_index]);
            ctx->er->addError(e);
            return false;
        }
    }

    llvm::Function *existing_llvm_fn =
        units->top()->module->getFunction(symbol.c_str());
    if (existing_llvm_fn) {
        Function *existing_fn = ctx->getFunction(symbol.c_str(), NULL,
                                                 NULL, 0);
        if (existing_fn && !existing_fn->isEqualTo(fn)) {
            Error *e = new Error(RedeclarationOfFunctionOrMacro,
                                 node, name);
            ctx->er->addError(e);
            return false;
        }
        if (existing_fn && !existing_fn->attrsAreEqual(fn)) {
            Error *e = new Error(AttributesOfDeclAndDefAreDifferent,
                                 node, name);
            ctx->er->addError(e);
            return false;
        }
    }

    if (units->top()->once_tag.length() > 0) {
        fn->once_tag = units->top()->once_tag;
    }

    llvm::Constant *fnc =
        units->top()->module->getOrInsertFunction(symbol.c_str(), ft);

    llvm::Function *llvm_fn = llvm::dyn_cast<llvm::Function>(fnc);

    /* If llvm_fn is null, then the function already exists and the extant
     * function has a different prototype, so it's an invalid
     * redeclaration.  If llvm_fn is not null, but has content, then it's an
     * invalid redefinition. */

    if (!llvm_fn || llvm_fn->size()) {
        Error *e = new Error(RedeclarationOfFunctionOrMacro, node, name);
        ctx->er->addError(e);
        return false;
    }

    if (always_inline) {
#if D_LLVM_VERSION_MINOR == 2
        llvm_fn->addFnAttr(llvm::Attributes::AlwaysInline);
#else
        llvm_fn->addFnAttr(llvm::Attribute::AlwaysInline);
#endif
    }

    llvm_fn->setCallingConv(llvm::CallingConv::C);
    llvm_fn->setLinkage(
        (lst->size() == (unsigned) (next_index + 2))
            ? ctx->toLLVMLinkage(override_linkage)
            : ctx->toLLVMLinkage(linkage)
    );

    linkVariablesToFunction(&fn_args_internal, llvm_fn);

    llvm::Value *llvm_return_value = NULL;
    if (ret_type->is_retval) {
        llvm::Function::arg_iterator llvm_arg_iter = llvm_fn->arg_begin();
        std::advance(llvm_arg_iter, fn->parameters.size());
        llvm_return_value = llvm_arg_iter;
        llvm_return_value->setName("retval");
    }

    fn->llvm_function = llvm_fn;
    if (!ctx->ns()->addFunction(name, fn, node)) {
        return false;
    }

    if (new_fn) {
        *new_fn = fn;
    }

    /* If the list has only four arguments, function is a
     * declaration and you can return straightaway. */

    if (lst->size() == (unsigned) (next_index + 2)) {
        return true;
    }

    ctx->activateAnonymousNamespace();
    anon_name = ctx->ns()->name;

    units->top()->pushGlobalFunction(fn);
    FormProcBodyParse(units, node, fn, llvm_fn, (next_index + 2),
                      is_anonymous, llvm_return_value);
    units->top()->popGlobalFunction();

    if (!strcmp(name, "main")
            && (!strcmp(SYSTEM_NAME, "Darwin")
             || !strcmp(SYSTEM_NAME, "FreeBSD"))
            && ctx->getVariable("stdin")
            && ctx->getVariable("stdout")
            && ctx->getVariable("stderr")) {

        std::vector<llvm::Value *> call_args;
        std::vector<Type *> params;
        Function *ic =
            ctx->getFunction("init-channels", &params, NULL, 0);
        assert(ic && ic->llvm_function &&
               "cannot find init-channels function");

        llvm::Function::iterator i = llvm_fn->begin();
        llvm::BasicBlock *b = i;

        if (b->empty()) {
            llvm::CallInst::Create(
                ic->llvm_function,
                llvm::ArrayRef<llvm::Value*>(call_args),
                "",
                b
            );
        } else {
            llvm::Instruction *fnp = b->getFirstNonPHI();
            if (fnp) {
                llvm::CallInst::Create(
                    ic->llvm_function,
                    llvm::ArrayRef<llvm::Value*>(call_args),
                    "",
                    fnp
                );
            } else {
                llvm::CallInst::Create(
                    ic->llvm_function,
                    llvm::ArrayRef<llvm::Value*>(call_args),
                    "",
                    b
                );
            }
        }
    }

    ctx->deactivateNamespace(anon_name.c_str());

    return true;
}
}
<commit_msg>[master] tidying<commit_after>#include "Function.h"
#include "../../Units/Units.h"
#include "../../Node/Node.h"
#include "../../ParseResult/ParseResult.h"
#include "../../Function/Function.h"
#include "../../CoreForms/CoreForms.h"
#include "../Linkage/Linkage.h"
#include "../Type/Type.h"
#include "../Function/Function.h"
#include "../ProcBody/ProcBody.h"
#include "../Parameter/Parameter.h"
#include "../Utils/Utils.h"
#include "../../llvm_Function.h"
#include "Config.h"

using namespace dale::ErrorInst;

namespace dale
{
bool
parseFunctionAttributes(Context *ctx, std::vector<Node *> *attr_list,
                        bool *always_inline, bool *cto)
{
    for (std::vector<Node*>::iterator b = (attr_list->begin() + 1),
                                      e = attr_list->end();
            b != e;
            ++b) {
        if ((*b)->is_list) {
            Error *e = new Error(InvalidAttribute, (*b));
            ctx->er->addError(e);
            return false;
        }
        if (!((*b)->token->str_value.compare("inline"))) {
            *always_inline = true;
        } else if (!((*b)->token->str_value.compare("cto"))) {
            *cto = true;
        } else {
            Error *e = new Error(InvalidAttribute, (*b));
            ctx->er->addError(e);
            return false;
        }
    }
    return true;
}

bool
parseParameters(Units *units, Node *args_node,
               std::vector<Variable *> *fn_args_internal)
{
    Context *ctx = units->top()->ctx;
    std::vector<Node *> *args = args_node->list;

    for (std::vector<Node *>::iterator b = args->begin(),
                                       e = args->end();
            b != e;
            ++b) {
        Variable *var = new Variable();
        var->type = NULL;

        FormParameterParse(units, var, (*b), false, false, true);
        if (var->type == NULL) {
            delete var;
            return false;
        }

        if (var->type->is_array) {
            delete var;
            Error *e = new Error(ArraysCannotBeFunctionParameters,
                                 (*b));
            ctx->er->addError(e);
            return false;
        }

        if (var->type->base_type == BaseType::Void) {
            delete var;
            if (args->size() != 1) {
                Error *e = new Error(VoidMustBeTheOnlyParameter,
                                     args_node);
                ctx->er->addError(e);
                return false;
            }
            break;
        }

        if (var->type->base_type == BaseType::VarArgs) {
            if ((args->end() - b) != 1) {
                delete var;
                Error *e = new Error(VarArgsMustBeLastParameter,
                                     args_node);
                ctx->er->addError(e);
                return false;
            }
            fn_args_internal->push_back(var);
            break;
        }

        if (var->type->is_function) {
            delete var;
            Error *e = new Error(NonPointerFunctionParameter, (*b));
            ctx->er->addError(e);
            return false;
        }

        fn_args_internal->push_back(var);
    }

    return true;
}

void
addInitChannelsCall(Context *ctx, llvm::Function *llvm_fn)
{
    std::vector<llvm::Value *> call_args;
    std::vector<Type *> params;
    Function *ic =
        ctx->getFunction("init-channels", &params, NULL, 0);
    assert(ic && ic->llvm_function &&
            "cannot find init-channels function");

    llvm::Function::iterator i = llvm_fn->begin();
    llvm::BasicBlock *b = i;

    if (b->empty()) {
        llvm::CallInst::Create(
            ic->llvm_function,
            llvm::ArrayRef<llvm::Value*>(call_args),
            "",
            b
        );
    } else {
        llvm::Instruction *fnp = b->getFirstNonPHI();
        if (fnp) {
            llvm::CallInst::Create(
                ic->llvm_function,
                llvm::ArrayRef<llvm::Value*>(call_args),
                "",
                fnp
            );
        } else {
            llvm::CallInst::Create(
                ic->llvm_function,
                llvm::ArrayRef<llvm::Value*>(call_args),
                "",
                b
            );
        }
    }
}

bool
parametersToLLVMTypes(Context *ctx, std::vector<Variable *> *parameters,
                      std::vector<llvm::Type *> *types)
{
    for (std::vector<Variable *>::iterator b = parameters->begin(),
                                           e = parameters->end();
            b != e;
            ++b) {
        Type *type = (*b)->type;
        if (type->is_reference) {
            type = ctx->tr->getPointerType(type);
        }
        if (type->base_type == BaseType::VarArgs) {
            break;
        }
        llvm::Type *llvm_type = ctx->toLLVMType(type, NULL, false);
        if (!llvm_type) {
            return false;
        }
        types->push_back(llvm_type);
    }

    return true;
}

Type *
parseReturnType(Units *units, Context *ctx,
                std::vector<Variable *> *parameters, Node *node)
{
    /* For return type parsing, activate an anonymous namespace and
     * add all of the function parameters to it.  This is so that if
     * the return type uses a macro that depends on one of those
     * parameters, it will work properly. */

    ctx->activateAnonymousNamespace();
    std::string anon_name = ctx->ns()->name;

    for (std::vector<Variable *>::iterator b = parameters->begin(),
                                           e = parameters->end();
            b != e;
            ++b) {
        ctx->ns()->addVariable((*b)->name.c_str(), (*b));
    }

    Type *ret_type = FormTypeParse(units, node, false, false, false, true);

    ctx->deactivateNamespace(anon_name.c_str());

    if (!ret_type) {
        return NULL;
    }

    if (ret_type->is_array) {
        Error *e = new Error(ReturnTypesCannotBeArrays, node);
        ctx->er->addError(e);
        return NULL;
    }

    return ret_type;
}

bool
FormFunctionParse(Units *units, Node *node, const char *name,
                  Function **new_fn, int override_linkage,
                  bool is_anonymous)
{
    Context *ctx = units->top()->ctx;

    if (!name) {
        Node *name_node = (*(node->list))[1];
        name = name_node->token->str_value.c_str();
        node = (*(node->list))[2];
    }

    if (!is_anonymous) {
        units->prefunction_ns = ctx->ns();
    }

    if (CoreForms::existsNoOverride(name)) {
        Error *e = new Error(ThisCoreFormCannotBeOverridden, node);
        ctx->er->addError(e);
        return false;
    }

    std::vector<Node *> *lst = node->list;

    if (lst->size() < 4) {
        Error *e = new Error(IncorrectMinimumNumberOfArgs, node, "fn",
                             "3", (lst->size() - 1));
        ctx->er->addError(e);
        return false;
    }

    int next_index = 1;
    bool always_inline = false;
    bool cto = units->cto;

    /* Whole modules, as well as specific functions, can be declared
     * compile-time-only.  If global CTO is enabled, that overrides
     * the absence of a CTO attribute here.  However, if a function
     * explicitly declares that it is CTO, that will override the
     * global setting. */

    Node *test = ((*lst)[next_index]);
    if (test->is_list
            && (*test->list)[0]->is_token
            && !((*test->list)[0]->token->str_value.compare("attr"))) {
        bool res = parseFunctionAttributes(ctx, test->list,
                                           &always_inline, &cto);
        if (!res) {
            return false;
        }
        ++next_index;
    }

    int linkage =
        (override_linkage)
            ? override_linkage
            : FormLinkageParse(ctx, (*lst)[next_index]);
    if (!linkage) {
        return false;
    }
    if (!override_linkage) {
        ++next_index;
    }

    /* The return type is not parsed yet, because it may depend on the
     * types of the function parameters. */

    int return_type_index = next_index;

    Node *args_node = (*lst)[next_index + 1];
    if (!args_node->is_list) {
        Error *e = new Error(UnexpectedElement, args_node,
                             "list", "parameters", "symbol");
        ctx->er->addError(e);
        return false;
    }

    std::vector<Variable *> fn_args_internal;
    bool res = parseParameters(units, args_node, &fn_args_internal);
    if (!res) {
        return false;
    }
    bool varargs = false;
    if (fn_args_internal.size()
            && (fn_args_internal.back()->type->base_type == BaseType::VarArgs)) {
        varargs = true;
    }

    std::vector<llvm::Type*> fn_args;
    res = parametersToLLVMTypes(ctx, &fn_args_internal, &fn_args);
    if (!res) {
        return false;
    }

    Type *ret_type = parseReturnType(units, ctx, &fn_args_internal,
                                     (*lst)[return_type_index]);
    if (ret_type == NULL) {
        return false;
    }

    llvm::Type *llvm_ret_type = ctx->toLLVMType(ret_type, NULL, true);
    if (!llvm_ret_type) {
        return false;
    }

    /* Create the LLVM function type.  If the retval attribute is
     * present, then the LLVM function type will have a return type of
     * void, and a pointer to a value of the return type will be the
     * final parameter. */

    if (ret_type->is_retval) {
        fn_args.push_back(ctx->toLLVMType(ctx->tr->getPointerType(ret_type),
                                          NULL, true));
        llvm_ret_type = ctx->toLLVMType(ctx->tr->getBasicType(BaseType::Void),
                                        NULL, true);
    }
    llvm::FunctionType *ft = getFunctionType(llvm_ret_type, fn_args,
                                             varargs);

    std::string symbol;
    ctx->ns()->functionNameToSymbol(name, &symbol, linkage,
                                    &fn_args_internal);

    Function *fn = new Function(ret_type, &fn_args_internal, NULL, 0,
                                 &symbol, always_inline);
    fn->linkage = linkage;
    fn->cto = cto;
    if (!strcmp(name, "setf-copy") || !strcmp(name, "setf-assign")) {
        fn->is_setf_fn = true;
    } else if (!strcmp(name, "destroy")) {
        fn->is_destructor = true;
    }

    if (fn->is_setf_fn) {
        if (!ret_type->isEqualTo(ctx->tr->type_bool)) {
            Error *e = new Error(SetfOverridesMustReturnBool,
                                 (*lst)[return_type_index]);
            ctx->er->addError(e);
            return false;
        }
    }

    llvm::Function *existing_llvm_fn =
        units->top()->module->getFunction(symbol.c_str());
    if (existing_llvm_fn) {
        Function *existing_fn = ctx->getFunction(symbol.c_str(), NULL,
                                                 NULL, 0);
        if (existing_fn && !existing_fn->isEqualTo(fn)) {
            Error *e = new Error(RedeclarationOfFunctionOrMacro,
                                 node, name);
            ctx->er->addError(e);
            return false;
        }
        if (existing_fn && !existing_fn->attrsAreEqual(fn)) {
            Error *e = new Error(AttributesOfDeclAndDefAreDifferent,
                                 node, name);
            ctx->er->addError(e);
            return false;
        }
    }

    if (units->top()->once_tag.length() > 0) {
        fn->once_tag = units->top()->once_tag;
    }

    llvm::Constant *fnc =
        units->top()->module->getOrInsertFunction(symbol.c_str(), ft);

    llvm::Function *llvm_fn = llvm::dyn_cast<llvm::Function>(fnc);

    /* If llvm_fn is null, then the function already exists and the extant
     * function has a different prototype, so it's an invalid
     * redeclaration.  If llvm_fn is not null, but has content, then it's an
     * invalid redefinition. */

    if (!llvm_fn || llvm_fn->size()) {
        Error *e = new Error(RedeclarationOfFunctionOrMacro, node, name);
        ctx->er->addError(e);
        return false;
    }

    if (always_inline) {
#if D_LLVM_VERSION_MINOR == 2
        llvm_fn->addFnAttr(llvm::Attributes::AlwaysInline);
#else
        llvm_fn->addFnAttr(llvm::Attribute::AlwaysInline);
#endif
    }

    llvm_fn->setCallingConv(llvm::CallingConv::C);
    llvm_fn->setLinkage(
        (lst->size() == (unsigned) (next_index + 2))
            ? ctx->toLLVMLinkage(override_linkage)
            : ctx->toLLVMLinkage(linkage)
    );

    linkVariablesToFunction(&fn_args_internal, llvm_fn);

    llvm::Value *llvm_return_value = NULL;
    if (ret_type->is_retval) {
        llvm::Function::arg_iterator llvm_arg_iter = llvm_fn->arg_begin();
        std::advance(llvm_arg_iter, fn->parameters.size());
        llvm_return_value = llvm_arg_iter;
        llvm_return_value->setName("retval");
    }

    fn->llvm_function = llvm_fn;
    if (!ctx->ns()->addFunction(name, fn, node)) {
        return false;
    }

    if (new_fn) {
        *new_fn = fn;
    }

    /* If the list has only four arguments, function is a
     * declaration and you can return straightaway. */

    if (lst->size() == (unsigned) (next_index + 2)) {
        return true;
    }

    ctx->activateAnonymousNamespace();
    std::string anon_name = ctx->ns()->name;

    units->top()->pushGlobalFunction(fn);
    FormProcBodyParse(units, node, fn, llvm_fn, (next_index + 2),
                      is_anonymous, llvm_return_value);
    units->top()->popGlobalFunction();

    if (!strcmp(name, "main")
            && (!strcmp(SYSTEM_NAME, "Darwin")
             || !strcmp(SYSTEM_NAME, "FreeBSD"))
            && ctx->getVariable("stdin")
            && ctx->getVariable("stdout")
            && ctx->getVariable("stderr")) {
        addInitChannelsCall(ctx, llvm_fn);
    }

    ctx->deactivateNamespace(anon_name.c_str());

    return true;
}
}
<|endoftext|>


<commit_before>#pragma once
#include "handle.hpp"
#include "vertex.hpp"

namespace rs {
	namespace vertex {
		//! ScreenRect描画用頂点
		struct screen {
			spn::Vec2	pos;
			spn::Vec2	tex;
		};
	}
	namespace vdecl {
		struct screen {};
	}
}
DefineVDecl(::rs::vdecl::screen)

namespace rs {
	struct DrawTag;
	namespace util {
		//! 画面全体を覆う矩形ポリゴン (ポストエフェクト用)
		class ScreenRect {
			private:
				static WVb		s_wVb;
				static WIb		s_wIb;
				HLVb			_hlVb;
				HLIb			_hlIb;
			public:
				ScreenRect();
				void exportDrawTag(DrawTag& tag) const;
				void draw(GLEffect& e) const;
		};
	}
}
<commit_msg>util/screenrect.hpp インクルードパスを修正<commit_after>#pragma once
#include "../handle.hpp"
#include "../vertex.hpp"

namespace rs {
	namespace vertex {
		//! ScreenRect描画用頂点
		struct screen {
			spn::Vec2	pos;
			spn::Vec2	tex;
		};
	}
	namespace vdecl {
		struct screen {};
	}
}
DefineVDecl(::rs::vdecl::screen)

namespace rs {
	struct DrawTag;
	namespace util {
		//! 画面全体を覆う矩形ポリゴン (ポストエフェクト用)
		class ScreenRect {
			private:
				static WVb		s_wVb;
				static WIb		s_wIb;
				HLVb			_hlVb;
				HLIb			_hlIb;
			public:
				ScreenRect();
				void exportDrawTag(DrawTag& tag) const;
				void draw(GLEffect& e) const;
		};
	}
}
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2015, Daiki Maekawa and Chiba Institute of Technology.
 * 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.
 * 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 <ros/ros.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/MarkerArray.h>
#include <fulanghua_srvs/Pose.h>

#include <yaml-cpp/yaml.h>

#include <vector>
#include <fstream>
#include <string>
#include <exception>
#include <math.h>
#include <limits>

#ifdef NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
    i = node.as<T>();
}
#endif

class SwitchRunningStatus : public std::exception {
public:
    SwitchRunningStatus() : std::exception() { }
};

class WaypointsNavigation{
public:
    WaypointsNavigation() :
        has_activate_(false),
        move_base_action_("move_base", true),
        rate_(10),
        last_moved_time_(0)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
        {
            ROS_INFO("Waiting...");
        }

        ros::NodeHandle private_nh("~");
        private_nh.param("robot_frame", robot_frame_, std::string("/base_link"));
        private_nh.param("world_frame", world_frame_, std::string("/map"));

        double max_update_rate;
        private_nh.param("max_update_rate", max_update_rate, 10.0);
        rate_ = ros::Rate(max_update_rate);
        std::string filename = "";
        private_nh.param("filename", filename, filename);
        if(filename != ""){
            ROS_INFO_STREAM("Read waypoints data from " << filename);
            if(!readFile(filename)) {
                ROS_ERROR("Failed loading waypoints file");
            }
            current_waypoint_ = waypoints_.begin();
        } else {
            ROS_ERROR("waypoints file doesn't have name");
        }

        ros::NodeHandle nh;
        start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
        suspend_server_ = nh.advertiseService("suspend_wp_pose", &WaypointsNavigation::suspendPoseCallback, this);
        resume_server_ = nh.advertiseService("resume_wp_pose", &WaypointsNavigation::resumePoseCallback, this);
        cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
        marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);
        clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    }

    bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) {
        if(has_activate_) {
            response.success = false;
            return false;
        }

        std_srvs::Empty empty;
        while(!clear_costmaps_srv_.call(empty)) {
            ROS_WARN("Resend clear costmap service");
            sleep();
        }

        current_waypoint_ = waypoints_.begin();
        has_activate_ = true;
        response.success = true;
        return true;
    }

    bool resumePoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(has_activate_) {
            response.status = false;
            return false;
        }

        std_srvs::Empty empty;
        clear_costmaps_srv_.call(empty);
        //move_base_action_.cancelAllGoals();

        ///< @todo calculating metric with request orientation
        double min_dist = std::numeric_limits<double>::max();
        for(std::vector<geometry_msgs::PointStamped>::iterator it = waypoints_.begin(); it != waypoints_.end()-1; it++) {
            double dist = hypot(it->point.x - request.pose.position.x, it->point.y - request.pose.position.y);
            if(dist < min_dist) {
                min_dist = dist;
                current_waypoint_ = it;
            }
        }

        response.status = true;
        has_activate_ = true;

        return true;
    }

    bool suspendPoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(!has_activate_) {
            response.status = false;
            return false;
        }

        //move_base_action_.cancelAllGoals();
        startNavigationGL(request.pose);
        while(!navigationFinished() && ros::ok()) {
            sleep();
        }
        response.status = true;
        has_activate_ = false;

        return true;
    }

    void cmdVelCallback(const geometry_msgs::Twist &msg){
        if(msg.linear.x > -0.001 && msg.linear.x < 0.001   &&
           msg.linear.y > -0.001 && msg.linear.y < 0.001   &&
           msg.linear.z > -0.001 && msg.linear.z < 0.001   &&
           msg.angular.x > -0.001 && msg.angular.x < 0.001 &&
           msg.angular.y > -0.001 && msg.angular.y < 0.001 &&
           msg.angular.z > -0.001 && msg.angular.z < 0.001){

            ROS_INFO("command velocity all zero");
        }else{
            last_moved_time_ = ros::Time::now().toSec();
        }
    }

    bool readFile(const std::string &filename){
        waypoints_.clear();
        try{
            std::ifstream ifs(filename.c_str(), std::ifstream::in);
            if(ifs.good() == false){
                return false;
            }

            YAML::Node node;

            #ifdef NEW_YAMLCPP
                node = YAML::Load(ifs);
            #else
                YAML::Parser parser(ifs);
                parser.GetNextDocument(node);
            #endif

            #ifdef NEW_YAMLCPP
                const YAML::Node &wp_node_tmp = node["waypoints"];
                const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
            #else
                const YAML::Node *wp_node = node.FindValue("waypoints");
            #endif

            if(wp_node != NULL){
                for(int i=0; i < wp_node->size(); i++){
                    geometry_msgs::PointStamped point;

                    (*wp_node)[i]["point"]["x"] >> point.point.x;
                    (*wp_node)[i]["point"]["y"] >> point.point.y;
                    (*wp_node)[i]["point"]["z"] >> point.point.z;

                    waypoints_.push_back(point);

                }
            }else{
                return false;
            }

            #ifdef NEW_YAMLCPP
                const YAML::Node &fp_node_tmp = node["finish_pose"];
                const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;
            #else
                const YAML::Node *fp_node = node.FindValue("finish_pose");
            #endif

            if(fp_node != NULL){
                (*fp_node)["pose"]["position"]["x"] >> finish_pose_.position.x;
                (*fp_node)["pose"]["position"]["y"] >> finish_pose_.position.y;
                (*fp_node)["pose"]["position"]["z"] >> finish_pose_.position.z;

                (*fp_node)["pose"]["orientation"]["x"] >> finish_pose_.orientation.x;
                (*fp_node)["pose"]["orientation"]["y"] >> finish_pose_.orientation.y;
                (*fp_node)["pose"]["orientation"]["z"] >> finish_pose_.orientation.z;
                (*fp_node)["pose"]["orientation"]["w"] >> finish_pose_.orientation.w;
            }else{
                return false;
            }

        }catch(YAML::ParserException &e){
            return false;

        }catch(YAML::RepresentationException &e){
            return false;
        }

        return true;
    }

    bool shouldSendGoal(){
        bool ret = true;
        actionlib::SimpleClientGoalState state = move_base_action_.getState();
        if((state != actionlib::SimpleClientGoalState::ACTIVE) &&
           (state != actionlib::SimpleClientGoalState::PENDING) &&
           (state != actionlib::SimpleClientGoalState::RECALLED) &&
           (state != actionlib::SimpleClientGoalState::PREEMPTED))
        {
            ret = false;
        }

        if(waypoints_.empty()){
            ret = false;
        }

        return ret;
    }

    bool navigationFinished(){
        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    }

    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8){
        tf::StampedTransform robot_gl = getRobotPosGL();

        const double wx = dest.x;
        const double wy = dest.y;
        const double rx = robot_gl.getOrigin().x();
        const double ry = robot_gl.getOrigin().y();
        const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));

        return dist < dist_err;
    }

    tf::StampedTransform getRobotPosGL(){
        tf::StampedTransform robot_gl;
        try{
            tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
        }catch(tf::TransformException &e){
            ROS_WARN_STREAM("tf::TransformException: " << e.what());
        }

        return robot_gl;
    }

    void sleep(){
        rate_.sleep();
        ros::spinOnce();
        publishMarkers();
    }

    void startNavigationGL(const geometry_msgs::Point &dest){
        geometry_msgs::Pose pose;
        pose.position = dest;
        pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
        startNavigationGL(pose);
    }

    void startNavigationGL(const geometry_msgs::Pose &dest){
        move_base_msgs::MoveBaseGoal move_base_goal;
        move_base_goal.target_pose.header.stamp = ros::Time::now();
        move_base_goal.target_pose.header.frame_id = world_frame_;
        move_base_goal.target_pose.pose.position = dest.position;
        move_base_goal.target_pose.pose.orientation = dest.orientation;

        move_base_action_.sendGoal(move_base_goal);
    }

    void publishMarkers(){
        visualization_msgs::MarkerArray markers_array;
        for(int i=0; i < waypoints_.size(); i++){
            visualization_msgs::Marker marker, label;
            marker.header.frame_id = world_frame_;
            marker.header.stamp = ros::Time::now();
            marker.scale.x = 0.2;
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;
            marker.pose.position.z = marker.scale.z / 2.0;
            marker.color.r = 0.8f;
            marker.color.g = 0.2f;
            marker.color.b = 0.2f;

            std::stringstream name;
            name << "waypoint " << i;
            marker.ns = name.str();
            marker.id = i;
            marker.pose.position.x = waypoints_[i].point.x;
            marker.pose.position.y = waypoints_[i].point.y;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.color.a = 1.0f;
            markers_array.markers.push_back(marker);

            //ROS_INFO_STREAM("waypoints \n" << waypoints_[i]);
        }
        marker_pub_.publish(markers_array);
    }

    void run(){
        while(ros::ok()){
            try {
                if(has_activate_) {
                    ROS_INFO_STREAM("waypoints = " << *current_waypoint_);
                    startNavigationGL(current_waypoint_->point);
                    double start_nav_time = ros::Time::now().toSec();
                    while(!onNavigationPoint(current_waypoint_->point)) {
                        if(!has_activate_)
                            throw SwitchRunningStatus();

                        double time = ros::Time::now().toSec();
                        if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                            ROS_WARN("Resend the navigation goal.");
                            std_srvs::Empty empty;
                            clear_costmaps_srv_.call(empty);
                            startNavigationGL(current_waypoint_->point);
                            start_nav_time = time;
                        }
                        sleep();
                    }

                    if(current_waypoint_ != waypoints_.end()-1) {
                        current_waypoint_++;
                    } else {
                        startNavigationGL(finish_pose_);
                        while(!navigationFinished() && ros::ok()) sleep();
                        has_activate_ = false;
                    }
                }
            } catch(const SwitchRunningStatus &e) {
                ROS_INFO_STREAM("running status switched");
            }

            sleep();
        }
    }

private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    std::vector<geometry_msgs::PointStamped> waypoints_;
    std::vector<geometry_msgs::PointStamped>::iterator current_waypoint_;
    geometry_msgs::Pose finish_pose_;
    bool has_activate_;
    std::string robot_frame_, world_frame_;
    tf::TransformListener tf_listener_;
    ros::Rate rate_;
    ros::ServiceServer start_server_, suspend_server_, resume_server_;
    ros::Subscriber cmd_vel_sub_;
    ros::Publisher marker_pub_;
    ros::ServiceClient clear_costmaps_srv_;
    double last_moved_time_;

};

int main(int argc, char *argv[]){
    ros::init(argc, argv, ROS_PACKAGE_NAME);
    WaypointsNavigation w_nav;
    w_nav.run();

    return 0;
}
<commit_msg>fix the wrong range<commit_after>/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2015, Daiki Maekawa and Chiba Institute of Technology.
 * 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.
 * 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 <ros/ros.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/MarkerArray.h>
#include <fulanghua_srvs/Pose.h>

#include <yaml-cpp/yaml.h>

#include <vector>
#include <fstream>
#include <string>
#include <exception>
#include <math.h>
#include <limits>

#ifdef NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
    i = node.as<T>();
}
#endif

class SwitchRunningStatus : public std::exception {
public:
    SwitchRunningStatus() : std::exception() { }
};

class WaypointsNavigation{
public:
    WaypointsNavigation() :
        has_activate_(false),
        move_base_action_("move_base", true),
        rate_(10),
        last_moved_time_(0)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
        {
            ROS_INFO("Waiting...");
        }

        ros::NodeHandle private_nh("~");
        private_nh.param("robot_frame", robot_frame_, std::string("/base_link"));
        private_nh.param("world_frame", world_frame_, std::string("/map"));

        double max_update_rate;
        private_nh.param("max_update_rate", max_update_rate, 10.0);
        rate_ = ros::Rate(max_update_rate);
        std::string filename = "";
        private_nh.param("filename", filename, filename);
        if(filename != ""){
            ROS_INFO_STREAM("Read waypoints data from " << filename);
            if(!readFile(filename)) {
                ROS_ERROR("Failed loading waypoints file");
            }
            current_waypoint_ = waypoints_.begin();
        } else {
            ROS_ERROR("waypoints file doesn't have name");
        }

        ros::NodeHandle nh;
        start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this);
        suspend_server_ = nh.advertiseService("suspend_wp_pose", &WaypointsNavigation::suspendPoseCallback, this);
        resume_server_ = nh.advertiseService("resume_wp_pose", &WaypointsNavigation::resumePoseCallback, this);
        cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
        marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);
        clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    }

    bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) {
        if(has_activate_) {
            response.success = false;
            return false;
        }

        std_srvs::Empty empty;
        while(!clear_costmaps_srv_.call(empty)) {
            ROS_WARN("Resend clear costmap service");
            sleep();
        }

        current_waypoint_ = waypoints_.begin();
        has_activate_ = true;
        response.success = true;
        return true;
    }

    bool resumePoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(has_activate_) {
            response.status = false;
            return false;
        }

        std_srvs::Empty empty;
        clear_costmaps_srv_.call(empty);
        //move_base_action_.cancelAllGoals();

        ///< @todo calculating metric with request orientation
        double min_dist = std::numeric_limits<double>::max();
        for(std::vector<geometry_msgs::PointStamped>::iterator it = waypoints_.begin(); it != waypoints_.end(); it++) {
            double dist = hypot(it->point.x - request.pose.position.x, it->point.y - request.pose.position.y);
            if(dist < min_dist) {
                min_dist = dist;
                current_waypoint_ = it;
            }
        }

        response.status = true;
        has_activate_ = true;

        return true;
    }

    bool suspendPoseCallback(fulanghua_srvs::Pose::Request &request, fulanghua_srvs::Pose::Response &response) {
        if(!has_activate_) {
            response.status = false;
            return false;
        }

        //move_base_action_.cancelAllGoals();
        startNavigationGL(request.pose);
        while(!navigationFinished() && ros::ok()) {
            sleep();
        }
        response.status = true;
        has_activate_ = false;

        return true;
    }

    void cmdVelCallback(const geometry_msgs::Twist &msg){
        if(msg.linear.x > -0.001 && msg.linear.x < 0.001   &&
           msg.linear.y > -0.001 && msg.linear.y < 0.001   &&
           msg.linear.z > -0.001 && msg.linear.z < 0.001   &&
           msg.angular.x > -0.001 && msg.angular.x < 0.001 &&
           msg.angular.y > -0.001 && msg.angular.y < 0.001 &&
           msg.angular.z > -0.001 && msg.angular.z < 0.001){

            ROS_INFO("command velocity all zero");
        }else{
            last_moved_time_ = ros::Time::now().toSec();
        }
    }

    bool readFile(const std::string &filename){
        waypoints_.clear();
        try{
            std::ifstream ifs(filename.c_str(), std::ifstream::in);
            if(ifs.good() == false){
                return false;
            }

            YAML::Node node;

            #ifdef NEW_YAMLCPP
                node = YAML::Load(ifs);
            #else
                YAML::Parser parser(ifs);
                parser.GetNextDocument(node);
            #endif

            #ifdef NEW_YAMLCPP
                const YAML::Node &wp_node_tmp = node["waypoints"];
                const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
            #else
                const YAML::Node *wp_node = node.FindValue("waypoints");
            #endif

            if(wp_node != NULL){
                for(int i=0; i < wp_node->size(); i++){
                    geometry_msgs::PointStamped point;

                    (*wp_node)[i]["point"]["x"] >> point.point.x;
                    (*wp_node)[i]["point"]["y"] >> point.point.y;
                    (*wp_node)[i]["point"]["z"] >> point.point.z;

                    waypoints_.push_back(point);

                }
            }else{
                return false;
            }

            #ifdef NEW_YAMLCPP
                const YAML::Node &fp_node_tmp = node["finish_pose"];
                const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;
            #else
                const YAML::Node *fp_node = node.FindValue("finish_pose");
            #endif

            if(fp_node != NULL){
                (*fp_node)["pose"]["position"]["x"] >> finish_pose_.position.x;
                (*fp_node)["pose"]["position"]["y"] >> finish_pose_.position.y;
                (*fp_node)["pose"]["position"]["z"] >> finish_pose_.position.z;

                (*fp_node)["pose"]["orientation"]["x"] >> finish_pose_.orientation.x;
                (*fp_node)["pose"]["orientation"]["y"] >> finish_pose_.orientation.y;
                (*fp_node)["pose"]["orientation"]["z"] >> finish_pose_.orientation.z;
                (*fp_node)["pose"]["orientation"]["w"] >> finish_pose_.orientation.w;
            }else{
                return false;
            }

        }catch(YAML::ParserException &e){
            return false;

        }catch(YAML::RepresentationException &e){
            return false;
        }

        return true;
    }

    bool shouldSendGoal(){
        bool ret = true;
        actionlib::SimpleClientGoalState state = move_base_action_.getState();
        if((state != actionlib::SimpleClientGoalState::ACTIVE) &&
           (state != actionlib::SimpleClientGoalState::PENDING) &&
           (state != actionlib::SimpleClientGoalState::RECALLED) &&
           (state != actionlib::SimpleClientGoalState::PREEMPTED))
        {
            ret = false;
        }

        if(waypoints_.empty()){
            ret = false;
        }

        return ret;
    }

    bool navigationFinished(){
        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    }

    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8){
        tf::StampedTransform robot_gl = getRobotPosGL();

        const double wx = dest.x;
        const double wy = dest.y;
        const double rx = robot_gl.getOrigin().x();
        const double ry = robot_gl.getOrigin().y();
        const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));

        return dist < dist_err;
    }

    tf::StampedTransform getRobotPosGL(){
        tf::StampedTransform robot_gl;
        try{
            tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
        }catch(tf::TransformException &e){
            ROS_WARN_STREAM("tf::TransformException: " << e.what());
        }

        return robot_gl;
    }

    void sleep(){
        rate_.sleep();
        ros::spinOnce();
        publishMarkers();
    }

    void startNavigationGL(const geometry_msgs::Point &dest){
        geometry_msgs::Pose pose;
        pose.position = dest;
        pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
        startNavigationGL(pose);
    }

    void startNavigationGL(const geometry_msgs::Pose &dest){
        move_base_msgs::MoveBaseGoal move_base_goal;
        move_base_goal.target_pose.header.stamp = ros::Time::now();
        move_base_goal.target_pose.header.frame_id = world_frame_;
        move_base_goal.target_pose.pose.position = dest.position;
        move_base_goal.target_pose.pose.orientation = dest.orientation;

        move_base_action_.sendGoal(move_base_goal);
    }

    void publishMarkers(){
        visualization_msgs::MarkerArray markers_array;
        for(int i=0; i < waypoints_.size(); i++){
            visualization_msgs::Marker marker, label;
            marker.header.frame_id = world_frame_;
            marker.header.stamp = ros::Time::now();
            marker.scale.x = 0.2;
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;
            marker.pose.position.z = marker.scale.z / 2.0;
            marker.color.r = 0.8f;
            marker.color.g = 0.2f;
            marker.color.b = 0.2f;

            std::stringstream name;
            name << "waypoint " << i;
            marker.ns = name.str();
            marker.id = i;
            marker.pose.position.x = waypoints_[i].point.x;
            marker.pose.position.y = waypoints_[i].point.y;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.color.a = 1.0f;
            markers_array.markers.push_back(marker);

            //ROS_INFO_STREAM("waypoints \n" << waypoints_[i]);
        }
        marker_pub_.publish(markers_array);
    }

    void run(){
        while(ros::ok()){
            try {
                if(has_activate_) {
                    ROS_INFO_STREAM("waypoints = " << *current_waypoint_);
                    startNavigationGL(current_waypoint_->point);
                    double start_nav_time = ros::Time::now().toSec();
                    while(!onNavigationPoint(current_waypoint_->point)) {
                        if(!has_activate_)
                            throw SwitchRunningStatus();

                        double time = ros::Time::now().toSec();
                        if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) {
                            ROS_WARN("Resend the navigation goal.");
                            std_srvs::Empty empty;
                            clear_costmaps_srv_.call(empty);
                            startNavigationGL(current_waypoint_->point);
                            start_nav_time = time;
                        }
                        sleep();
                    }

                    if(current_waypoint_ != waypoints_.end()-1) {
                        current_waypoint_++;
                    } else {
                        startNavigationGL(finish_pose_);
                        while(!navigationFinished() && ros::ok()) sleep();
                        has_activate_ = false;
                    }
                }
            } catch(const SwitchRunningStatus &e) {
                ROS_INFO_STREAM("running status switched");
            }

            sleep();
        }
    }

private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    std::vector<geometry_msgs::PointStamped> waypoints_;
    std::vector<geometry_msgs::PointStamped>::iterator current_waypoint_;
    geometry_msgs::Pose finish_pose_;
    bool has_activate_;
    std::string robot_frame_, world_frame_;
    tf::TransformListener tf_listener_;
    ros::Rate rate_;
    ros::ServiceServer start_server_, suspend_server_, resume_server_;
    ros::Subscriber cmd_vel_sub_;
    ros::Publisher marker_pub_;
    ros::ServiceClient clear_costmaps_srv_;
    double last_moved_time_;

};

int main(int argc, char *argv[]){
    ros::init(argc, argv, ROS_PACKAGE_NAME);
    WaypointsNavigation w_nav;
    w_nav.run();

    return 0;
}
<|endoftext|>


<commit_before>/*
    avatarselectorwidget.cpp - Widget to manage and select user avatar

    Copyright (c) 2007      by Michaël Larouche      <larouche@kde.org>

    Kopete    (c) 2002-2007 by the Kopete developers <kopete-devel@kde.org>

    *************************************************************************
    *                                                                       *
    * This library is free software; you can redistribute it and/or         *
    * modify it under the terms of the GNU Lesser General Public            *
    * License as published by the Free Software Foundation; either          *
    * version 2 of the License, or (at your option) any later version.      *
    *                                                                       *
    *************************************************************************
*/
#include "avatarselectorwidget.h"

// Qt includes
#include <QtGui/QListWidget>
#include <QtGui/QListWidgetItem>
#include <QtGui/QIcon>

// KDE includes
#include <kdebug.h>
#include <klocale.h>
#include <kurl.h>
#include <kfiledialog.h>
#include <kpixmapregionselectordialog.h>

#include "ui_avatarselectorwidget.h"

namespace Kopete
{
namespace UI
{

class AvatarSelectorWidgetItem : public QListWidgetItem
{
public:
	AvatarSelectorWidgetItem(QListWidget *parent)
	 : QListWidgetItem(parent, QListWidgetItem::UserType)
	{}

	void setAvatarEntry(Kopete::AvatarManager::AvatarEntry entry)
	{
		m_entry = entry;
		setText( entry.name );
		setIcon( QIcon(entry.path) );
	}

	Kopete::AvatarManager::AvatarEntry avatarEntry() const
	{
		return m_entry;
	}

private:
	Kopete::AvatarManager::AvatarEntry m_entry;
};

class AvatarSelectorWidget::Private
{
public:
	Private()
	 : selectedItem(0)
	{}

	Ui::AvatarSelectorWidget mainWidget;
	QListWidgetItem *selectedItem;

	void addItem(Kopete::AvatarManager::AvatarEntry entry);
};

AvatarSelectorWidget::AvatarSelectorWidget(QWidget *parent)
 : QWidget(parent), d(new Private)
{
	d->mainWidget.setupUi(this);

	// Connect signals/slots
	connect(d->mainWidget.buttonAddAvatar, SIGNAL(clicked()), this, SLOT(buttonAddAvatarClicked()));
	connect(d->mainWidget.buttonRemoveAvatar, SIGNAL(clicked()), this, SLOT(buttonRemoveAvatarClicked()));
	connect(d->mainWidget.listUserAvatar, SIGNAL(itemClicked(QListWidgetItem*)), this, SLOT(listSelectionChanged(QListWidgetItem*)));
	connect(d->mainWidget.listUserContact, SIGNAL(itemClicked(QListWidgetItem*)), this, SLOT(listSelectionChanged(QListWidgetItem*)));

	connect(Kopete::AvatarManager::self(), SIGNAL(avatarAdded(Kopete::AvatarManager::AvatarEntry)), this, SLOT(avatarAdded(Kopete::AvatarManager::AvatarEntry)));
	connect(Kopete::AvatarManager::self(), SIGNAL(avatarRemoved(Kopete::AvatarManager::AvatarEntry)), this, SLOT(avatarRemoved(Kopete::AvatarManager::AvatarEntry)));

	// List avatars in lists
	Kopete::AvatarQueryJob *queryJob = new Kopete::AvatarQueryJob(this);
	connect(queryJob, SIGNAL(result(KJob*)), this, SLOT(queryJobFinished(KJob*)));
	queryJob->setQueryFilter( Kopete::AvatarManager::All );

	queryJob->start();
}

AvatarSelectorWidget::~AvatarSelectorWidget()
{
	delete d;
}

Kopete::AvatarManager::AvatarEntry AvatarSelectorWidget::selectedEntry() const
{
	Kopete::AvatarManager::AvatarEntry result;

	if( d->selectedItem )
	{
		result = static_cast<AvatarSelectorWidgetItem*>(d->selectedItem)->avatarEntry();
	}

	return result;
}

void AvatarSelectorWidget::buttonAddAvatarClicked()
{
	KUrl imageUrl = KFileDialog::getImageOpenUrl( KUrl(), this );
	if( !imageUrl.isEmpty() )
	{
		// TODO: Download image
		if( !imageUrl.isLocalFile() )
		{
			d->mainWidget.labelErrorMessage->setText( i18n("You can only add avatar from local file.") );
			return;
		}

		// Crop the image
		QImage avatar = KPixmapRegionSelectorDialog::selectedImage( QPixmap(imageUrl.path()), 96, 96, this );

		QString imageName = imageUrl.fileName();

		Kopete::AvatarManager::AvatarEntry newEntry;
		// Remove extension from filename
		newEntry.name = imageName.left( imageName.lastIndexOf('.') );
		newEntry.image = avatar;
		newEntry.category = Kopete::AvatarManager::User;

		Kopete::AvatarManager::AvatarEntry addedEntry = Kopete::AvatarManager::self()->add( newEntry );
		if( addedEntry.path.isEmpty() )
		{
			d->mainWidget.labelErrorMessage->setText( i18n("Kopete can not add this new avatar because it could not save the avatar image in user directory.") );
		}
	}
}

void AvatarSelectorWidget::buttonRemoveAvatarClicked()
{
	// You can't remove from listUserContact, so we can always use listUserAvatar
	AvatarSelectorWidgetItem *selectedItem = static_cast<AvatarSelectorWidgetItem*>( d->mainWidget.listUserAvatar->selectedItems().first() );
	if( selectedItem )
	{
		if( !Kopete::AvatarManager::self()->remove( selectedItem->avatarEntry() ) )
		{
			d->mainWidget.labelErrorMessage->setText( i18n("Kopete can not remove selected avatar.") );
			kDebug(14010) << k_funcinfo << "Removing of avatar failed for unknown reason." << endl;
		}
	}
}

void AvatarSelectorWidget::queryJobFinished(KJob *job)
{
	Kopete::AvatarQueryJob *queryJob = static_cast<Kopete::AvatarQueryJob*>(job);
	if( !queryJob->error() )
	{
		QList<Kopete::AvatarManager::AvatarEntry> avatarList = queryJob->avatarList();
		Kopete::AvatarManager::AvatarEntry entry;
		foreach(entry, avatarList)
		{
			d->addItem(entry);
		}
	}
	else
	{
		d->mainWidget.labelErrorMessage->setText( queryJob->errorText() );
	}
}

void AvatarSelectorWidget::avatarAdded(Kopete::AvatarManager::AvatarEntry newEntry)
{
	d->addItem(newEntry);
}

void AvatarSelectorWidget::avatarRemoved(Kopete::AvatarManager::AvatarEntry entryRemoved)
{
	// Same here, avatar can be only removed from listUserAvatar
	QList<QListWidgetItem *> foundItems = d->mainWidget.listUserAvatar->findItems( entryRemoved.name, Qt::MatchContains );
	if( !foundItems.isEmpty() )
	{
		kDebug(14010) << k_funcinfo << "Removing " << entryRemoved.name << " from list." << endl;

		int deletedRow = d->mainWidget.listUserAvatar->row( foundItems.first() );
		QListWidgetItem *removedItem = d->mainWidget.listUserAvatar->takeItem( deletedRow );
		delete removedItem;

		int newRow = --deletedRow;
		if( newRow < 0 )
			newRow = 0;

		// Select the previous avatar in the list, thus selecting a new avatar
		// and deselecting the avatar being removed.
		d->mainWidget.listUserAvatar->setCurrentRow( newRow );
		// Force update
		listSelectionChanged( d->mainWidget.listUserAvatar->item(newRow) );
	}
}

void AvatarSelectorWidget::listSelectionChanged(QListWidgetItem *item)
{
	if( item )
	{
		d->mainWidget.labelAvatarImage->setPixmap( item->icon().pixmap(96, 96) );
		d->selectedItem = item;
	}

	// I know sender() is evil
	// Disable Remove Avatar button when selecting an item in listUserContact.
	// I don't know anyone who will want to remove avatar received from contacts.
	if( sender() == d->mainWidget.listUserContact )
	{
		d->mainWidget.buttonRemoveAvatar->setEnabled(false);
	}
	else
	{
		d->mainWidget.buttonRemoveAvatar->setEnabled(true);
	}
}

void AvatarSelectorWidget::Private::addItem(Kopete::AvatarManager::AvatarEntry entry)
{
	kDebug(14010) << k_funcinfo << "Entry(" << entry.name << "): " << entry.category << endl;

	QListWidget *listWidget  = 0;
	if( entry.category & Kopete::AvatarManager::User )
	{
		listWidget = mainWidget.listUserAvatar;
	}
	else if( entry.category & Kopete::AvatarManager::Contact )
	{
		listWidget = mainWidget.listUserContact;
	}
	else
	{
		return;
	}

	AvatarSelectorWidgetItem *item = new AvatarSelectorWidgetItem(listWidget);
	item->setAvatarEntry(entry);
}

} // Namespace Kopete::UI

} // Namespace Kopete

#include "avatarselectorwidget.moc"
<commit_msg>the api is getSelectedImage again<commit_after>/*
    avatarselectorwidget.cpp - Widget to manage and select user avatar

    Copyright (c) 2007      by Michaël Larouche      <larouche@kde.org>

    Kopete    (c) 2002-2007 by the Kopete developers <kopete-devel@kde.org>

    *************************************************************************
    *                                                                       *
    * This library is free software; you can redistribute it and/or         *
    * modify it under the terms of the GNU Lesser General Public            *
    * License as published by the Free Software Foundation; either          *
    * version 2 of the License, or (at your option) any later version.      *
    *                                                                       *
    *************************************************************************
*/
#include "avatarselectorwidget.h"

// Qt includes
#include <QtGui/QListWidget>
#include <QtGui/QListWidgetItem>
#include <QtGui/QIcon>

// KDE includes
#include <kdebug.h>
#include <klocale.h>
#include <kurl.h>
#include <kfiledialog.h>
#include <kpixmapregionselectordialog.h>

#include "ui_avatarselectorwidget.h"

namespace Kopete
{
namespace UI
{

class AvatarSelectorWidgetItem : public QListWidgetItem
{
public:
	AvatarSelectorWidgetItem(QListWidget *parent)
	 : QListWidgetItem(parent, QListWidgetItem::UserType)
	{}

	void setAvatarEntry(Kopete::AvatarManager::AvatarEntry entry)
	{
		m_entry = entry;
		setText( entry.name );
		setIcon( QIcon(entry.path) );
	}

	Kopete::AvatarManager::AvatarEntry avatarEntry() const
	{
		return m_entry;
	}

private:
	Kopete::AvatarManager::AvatarEntry m_entry;
};

class AvatarSelectorWidget::Private
{
public:
	Private()
	 : selectedItem(0)
	{}

	Ui::AvatarSelectorWidget mainWidget;
	QListWidgetItem *selectedItem;

	void addItem(Kopete::AvatarManager::AvatarEntry entry);
};

AvatarSelectorWidget::AvatarSelectorWidget(QWidget *parent)
 : QWidget(parent), d(new Private)
{
	d->mainWidget.setupUi(this);

	// Connect signals/slots
	connect(d->mainWidget.buttonAddAvatar, SIGNAL(clicked()), this, SLOT(buttonAddAvatarClicked()));
	connect(d->mainWidget.buttonRemoveAvatar, SIGNAL(clicked()), this, SLOT(buttonRemoveAvatarClicked()));
	connect(d->mainWidget.listUserAvatar, SIGNAL(itemClicked(QListWidgetItem*)), this, SLOT(listSelectionChanged(QListWidgetItem*)));
	connect(d->mainWidget.listUserContact, SIGNAL(itemClicked(QListWidgetItem*)), this, SLOT(listSelectionChanged(QListWidgetItem*)));

	connect(Kopete::AvatarManager::self(), SIGNAL(avatarAdded(Kopete::AvatarManager::AvatarEntry)), this, SLOT(avatarAdded(Kopete::AvatarManager::AvatarEntry)));
	connect(Kopete::AvatarManager::self(), SIGNAL(avatarRemoved(Kopete::AvatarManager::AvatarEntry)), this, SLOT(avatarRemoved(Kopete::AvatarManager::AvatarEntry)));

	// List avatars in lists
	Kopete::AvatarQueryJob *queryJob = new Kopete::AvatarQueryJob(this);
	connect(queryJob, SIGNAL(result(KJob*)), this, SLOT(queryJobFinished(KJob*)));
	queryJob->setQueryFilter( Kopete::AvatarManager::All );

	queryJob->start();
}

AvatarSelectorWidget::~AvatarSelectorWidget()
{
	delete d;
}

Kopete::AvatarManager::AvatarEntry AvatarSelectorWidget::selectedEntry() const
{
	Kopete::AvatarManager::AvatarEntry result;

	if( d->selectedItem )
	{
		result = static_cast<AvatarSelectorWidgetItem*>(d->selectedItem)->avatarEntry();
	}

	return result;
}

void AvatarSelectorWidget::buttonAddAvatarClicked()
{
	KUrl imageUrl = KFileDialog::getImageOpenUrl( KUrl(), this );
	if( !imageUrl.isEmpty() )
	{
		// TODO: Download image
		if( !imageUrl.isLocalFile() )
		{
			d->mainWidget.labelErrorMessage->setText( i18n("You can only add avatar from local file.") );
			return;
		}

		// Crop the image
		QImage avatar = KPixmapRegionSelectorDialog::getSelectedImage( QPixmap(imageUrl.path()), 96, 96, this );

		QString imageName = imageUrl.fileName();

		Kopete::AvatarManager::AvatarEntry newEntry;
		// Remove extension from filename
		newEntry.name = imageName.left( imageName.lastIndexOf('.') );
		newEntry.image = avatar;
		newEntry.category = Kopete::AvatarManager::User;

		Kopete::AvatarManager::AvatarEntry addedEntry = Kopete::AvatarManager::self()->add( newEntry );
		if( addedEntry.path.isEmpty() )
		{
			d->mainWidget.labelErrorMessage->setText( i18n("Kopete can not add this new avatar because it could not save the avatar image in user directory.") );
		}
	}
}

void AvatarSelectorWidget::buttonRemoveAvatarClicked()
{
	// You can't remove from listUserContact, so we can always use listUserAvatar
	AvatarSelectorWidgetItem *selectedItem = static_cast<AvatarSelectorWidgetItem*>( d->mainWidget.listUserAvatar->selectedItems().first() );
	if( selectedItem )
	{
		if( !Kopete::AvatarManager::self()->remove( selectedItem->avatarEntry() ) )
		{
			d->mainWidget.labelErrorMessage->setText( i18n("Kopete can not remove selected avatar.") );
			kDebug(14010) << k_funcinfo << "Removing of avatar failed for unknown reason." << endl;
		}
	}
}

void AvatarSelectorWidget::queryJobFinished(KJob *job)
{
	Kopete::AvatarQueryJob *queryJob = static_cast<Kopete::AvatarQueryJob*>(job);
	if( !queryJob->error() )
	{
		QList<Kopete::AvatarManager::AvatarEntry> avatarList = queryJob->avatarList();
		Kopete::AvatarManager::AvatarEntry entry;
		foreach(entry, avatarList)
		{
			d->addItem(entry);
		}
	}
	else
	{
		d->mainWidget.labelErrorMessage->setText( queryJob->errorText() );
	}
}

void AvatarSelectorWidget::avatarAdded(Kopete::AvatarManager::AvatarEntry newEntry)
{
	d->addItem(newEntry);
}

void AvatarSelectorWidget::avatarRemoved(Kopete::AvatarManager::AvatarEntry entryRemoved)
{
	// Same here, avatar can be only removed from listUserAvatar
	QList<QListWidgetItem *> foundItems = d->mainWidget.listUserAvatar->findItems( entryRemoved.name, Qt::MatchContains );
	if( !foundItems.isEmpty() )
	{
		kDebug(14010) << k_funcinfo << "Removing " << entryRemoved.name << " from list." << endl;

		int deletedRow = d->mainWidget.listUserAvatar->row( foundItems.first() );
		QListWidgetItem *removedItem = d->mainWidget.listUserAvatar->takeItem( deletedRow );
		delete removedItem;

		int newRow = --deletedRow;
		if( newRow < 0 )
			newRow = 0;

		// Select the previous avatar in the list, thus selecting a new avatar
		// and deselecting the avatar being removed.
		d->mainWidget.listUserAvatar->setCurrentRow( newRow );
		// Force update
		listSelectionChanged( d->mainWidget.listUserAvatar->item(newRow) );
	}
}

void AvatarSelectorWidget::listSelectionChanged(QListWidgetItem *item)
{
	if( item )
	{
		d->mainWidget.labelAvatarImage->setPixmap( item->icon().pixmap(96, 96) );
		d->selectedItem = item;
	}

	// I know sender() is evil
	// Disable Remove Avatar button when selecting an item in listUserContact.
	// I don't know anyone who will want to remove avatar received from contacts.
	if( sender() == d->mainWidget.listUserContact )
	{
		d->mainWidget.buttonRemoveAvatar->setEnabled(false);
	}
	else
	{
		d->mainWidget.buttonRemoveAvatar->setEnabled(true);
	}
}

void AvatarSelectorWidget::Private::addItem(Kopete::AvatarManager::AvatarEntry entry)
{
	kDebug(14010) << k_funcinfo << "Entry(" << entry.name << "): " << entry.category << endl;

	QListWidget *listWidget  = 0;
	if( entry.category & Kopete::AvatarManager::User )
	{
		listWidget = mainWidget.listUserAvatar;
	}
	else if( entry.category & Kopete::AvatarManager::Contact )
	{
		listWidget = mainWidget.listUserContact;
	}
	else
	{
		return;
	}

	AvatarSelectorWidgetItem *item = new AvatarSelectorWidgetItem(listWidget);
	item->setAvatarEntry(entry);
}

} // Namespace Kopete::UI

} // Namespace Kopete

#include "avatarselectorwidget.moc"
<|endoftext|>


<commit_before>//===----------------------------------------------------------------------===//
// LLVM 'GCCAS' UTILITY
//
//  This utility is designed to be used by the GCC frontend for creating
// bytecode files from it's intermediate llvm assembly.  The requirements for
// this utility are thus slightly different than that of the standard as util.
//
//===----------------------------------------------------------------------===//

#include "llvm/Module.h"
#include "llvm/Assembly/Parser.h"
#include "llvm/Transforms/CleanupGCCOutput.h"
#include "llvm/Transforms/LevelChange.h"
#include "llvm/Transforms/ConstantMerge.h"
#include "llvm/Transforms/ChangeAllocations.h"
#include "llvm/Transforms/Scalar/DCE.h"
#include "llvm/Transforms/Scalar/IndVarSimplify.h"
#include "llvm/Transforms/Scalar/InstructionCombining.h"
#include "llvm/Bytecode/WriteBytecodePass.h"
#include "Support/CommandLine.h"
#include <memory>
#include <fstream>
#include <string>

cl::String InputFilename ("", "Parse <arg> file, compile to bytecode",
                          cl::Required, "");
cl::String OutputFilename("o", "Override output filename", cl::NoFlags, "");

int main(int argc, char **argv) {
  cl::ParseCommandLineOptions(argc, argv, " llvm .s -> .o assembler for GCC\n");

  std::auto_ptr<Module> M;
  try {
    // Parse the file now...
    M.reset(ParseAssemblyFile(InputFilename));
  } catch (const ParseException &E) {
    cerr << E.getMessage() << endl;
    return 1;
  }

  if (M.get() == 0) {
    cerr << "assembly didn't read correctly.\n";
    return 1;
  }

  if (OutputFilename == "") {   // Didn't specify an output filename?
    std::string IFN = InputFilename;
    int Len = IFN.length();
    if (IFN[Len-2] == '.' && IFN[Len-1] == 's') {   // Source ends in .s?
      OutputFilename = std::string(IFN.begin(), IFN.end()-2);
    } else {
      OutputFilename = IFN;   // Append a .o to it
    }
    OutputFilename += ".o";
  }

  std::ofstream Out(OutputFilename.c_str(), ios::out);
  if (!Out.good()) {
    cerr << "Error opening " << OutputFilename << "!\n";
    return 1;
  }

  // In addition to just parsing the input from GCC, we also want to spiff it up
  // a little bit.  Do this now.
  //
  PassManager Passes;
  Passes.add(new DeadCodeElimination());       // Remove Dead code/vars
  Passes.add(new RaiseAllocations());          // call %malloc -> malloc inst
  Passes.add(new CleanupGCCOutput());          // Fix gccisms
  Passes.add(new InductionVariableSimplify()); // Simplify indvars
  Passes.add(new RaisePointerReferences());    // Eliminate casts
  Passes.add(new ConstantMerge());             // Merge dup global consts
  Passes.add(new InstructionCombining());      // Combine silly seq's
  Passes.add(new DeadCodeElimination());       // Remove Dead code/vars
  Passes.add(new WriteBytecodePass(&Out));     // Write bytecode to file...

  // Run our queue of passes all at once now, efficiently.
  Passes.run(M.get());
  return 0;
}

<commit_msg>Only run DeadInst elimination early, because it is quick and painless and pipelines well<commit_after>//===----------------------------------------------------------------------===//
// LLVM 'GCCAS' UTILITY
//
//  This utility is designed to be used by the GCC frontend for creating
// bytecode files from it's intermediate llvm assembly.  The requirements for
// this utility are thus slightly different than that of the standard as util.
//
//===----------------------------------------------------------------------===//

#include "llvm/Module.h"
#include "llvm/Assembly/Parser.h"
#include "llvm/Transforms/CleanupGCCOutput.h"
#include "llvm/Transforms/LevelChange.h"
#include "llvm/Transforms/ConstantMerge.h"
#include "llvm/Transforms/ChangeAllocations.h"
#include "llvm/Transforms/Scalar/DCE.h"
#include "llvm/Transforms/Scalar/IndVarSimplify.h"
#include "llvm/Transforms/Scalar/InstructionCombining.h"
#include "llvm/Bytecode/WriteBytecodePass.h"
#include "Support/CommandLine.h"
#include <memory>
#include <fstream>
#include <string>

cl::String InputFilename ("", "Parse <arg> file, compile to bytecode",
                          cl::Required, "");
cl::String OutputFilename("o", "Override output filename", cl::NoFlags, "");

int main(int argc, char **argv) {
  cl::ParseCommandLineOptions(argc, argv, " llvm .s -> .o assembler for GCC\n");

  std::auto_ptr<Module> M;
  try {
    // Parse the file now...
    M.reset(ParseAssemblyFile(InputFilename));
  } catch (const ParseException &E) {
    cerr << E.getMessage() << endl;
    return 1;
  }

  if (M.get() == 0) {
    cerr << "assembly didn't read correctly.\n";
    return 1;
  }

  if (OutputFilename == "") {   // Didn't specify an output filename?
    std::string IFN = InputFilename;
    int Len = IFN.length();
    if (IFN[Len-2] == '.' && IFN[Len-1] == 's') {   // Source ends in .s?
      OutputFilename = std::string(IFN.begin(), IFN.end()-2);
    } else {
      OutputFilename = IFN;   // Append a .o to it
    }
    OutputFilename += ".o";
  }

  std::ofstream Out(OutputFilename.c_str(), ios::out);
  if (!Out.good()) {
    cerr << "Error opening " << OutputFilename << "!\n";
    return 1;
  }

  // In addition to just parsing the input from GCC, we also want to spiff it up
  // a little bit.  Do this now.
  //
  PassManager Passes;
  Passes.add(new DeadInstElimination());       // Remove Dead code/vars
  Passes.add(new RaiseAllocations());          // call %malloc -> malloc inst
  Passes.add(new CleanupGCCOutput());          // Fix gccisms
  Passes.add(new InductionVariableSimplify()); // Simplify indvars
  Passes.add(new RaisePointerReferences());    // Eliminate casts
  Passes.add(new ConstantMerge());             // Merge dup global consts
  Passes.add(new InstructionCombining());      // Combine silly seq's
  Passes.add(new DeadCodeElimination());       // Remove Dead code/vars
  Passes.add(new WriteBytecodePass(&Out));     // Write bytecode to file...

  // Run our queue of passes all at once now, efficiently.
  Passes.run(M.get());
  return 0;
}

<|endoftext|>


<commit_before>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2007-2009 Soeren Sonnenburg
 * Copyright (C) 2007-2009 Fraunhofer Institute FIRST and Max-Planck-Society
 */

#include "lib/common.h"
#include "lib/io.h"
#include "distance/AttenuatedEuclidianDistance.h"
#include "features/Features.h"
#include "features/SimpleFeatures.h"

using namespace shogun;

CAttenuatedEuclidianDistance::CAttenuatedEuclidianDistance() : CRealDistance()
{
	init();
}

CAttenuatedEuclidianDistance::CAttenuatedEuclidianDistance(CSimpleFeatures<float64_t>* l, CSimpleFeatures<float64_t>* r)
: CRealDistance()
{
	init();
	init(l, r);
}

CAttenuatedEuclidianDistance::~CAttenuatedEuclidianDistance()
{
	cleanup();
}

bool CAttenuatedEuclidianDistance::init(CFeatures* l, CFeatures* r)
{
	CRealDistance::init(l, r);
	return true;
}

void CAttenuatedEuclidianDistance::cleanup()
{
}

float64_t CAttenuatedEuclidianDistance::compute(int32_t idx_a, int32_t idx_b)
{
	int32_t alen, blen;
	bool afree, bfree;
	float64_t result=0;

	float64_t* avec=((CSimpleFeatures<float64_t>*) lhs)->
		get_feature_vector(idx_a, alen, afree);
	float64_t* bvec=((CSimpleFeatures<float64_t>*) rhs)->
		get_feature_vector(idx_b, blen, bfree);
	ASSERT(alen==blen);

	for (int32_t i=0; i<alen; i++)
		result+=(CMath::abs(avec[i])*CMath::abs(bvec[i]))*CMath::pow(avec[i] - bvec[i],2);

	((CSimpleFeatures<float64_t>*) lhs)->free_feature_vector(avec, idx_a, afree);
	((CSimpleFeatures<float64_t>*) rhs)->free_feature_vector(bvec, idx_b, bfree);

	if (disable_sqrt)
		return result;

	return CMath::sqrt(result);
}

void CAttenuatedEuclidianDistance::init()
{
	disable_sqrt=false;

	m_parameters->add(&disable_sqrt, "disable_sqrt", "If sqrt shall not be applied.");
}
<commit_msg>Copyright issues.<commit_after>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2011 Miguel Angel Bautista
 * Copyright (C) 2011 Berlin Institute of Technology and Max Planck Society
 */

#include "lib/common.h"
#include "lib/io.h"
#include "distance/AttenuatedEuclidianDistance.h"
#include "features/Features.h"
#include "features/SimpleFeatures.h"

using namespace shogun;

CAttenuatedEuclidianDistance::CAttenuatedEuclidianDistance() : CRealDistance()
{
	init();
}

CAttenuatedEuclidianDistance::CAttenuatedEuclidianDistance(CSimpleFeatures<float64_t>* l, CSimpleFeatures<float64_t>* r)
: CRealDistance()
{
	init();
	init(l, r);
}

CAttenuatedEuclidianDistance::~CAttenuatedEuclidianDistance()
{
	cleanup();
}

bool CAttenuatedEuclidianDistance::init(CFeatures* l, CFeatures* r)
{
	CRealDistance::init(l, r);
	return true;
}

void CAttenuatedEuclidianDistance::cleanup()
{
}

float64_t CAttenuatedEuclidianDistance::compute(int32_t idx_a, int32_t idx_b)
{
	int32_t alen, blen;
	bool afree, bfree;
	float64_t result=0;

	float64_t* avec=((CSimpleFeatures<float64_t>*) lhs)->
		get_feature_vector(idx_a, alen, afree);
	float64_t* bvec=((CSimpleFeatures<float64_t>*) rhs)->
		get_feature_vector(idx_b, blen, bfree);
	ASSERT(alen==blen);

	for (int32_t i=0; i<alen; i++)
		result+=(CMath::abs(avec[i])*CMath::abs(bvec[i]))*CMath::pow(avec[i] - bvec[i],2);

	((CSimpleFeatures<float64_t>*) lhs)->free_feature_vector(avec, idx_a, afree);
	((CSimpleFeatures<float64_t>*) rhs)->free_feature_vector(bvec, idx_b, bfree);

	if (disable_sqrt)
		return result;

	return CMath::sqrt(result);
}

void CAttenuatedEuclidianDistance::init()
{
	disable_sqrt=false;

	m_parameters->add(&disable_sqrt, "disable_sqrt", "If sqrt shall not be applied.");
}
<|endoftext|>


<commit_before><commit_msg>support order of menu entries<commit_after><|endoftext|>


<commit_before>/** Copyright (C) 2016 European Spallation Source */

#include <libs/include/NMXEvent.h>
#include <libs/include/Timer.h>
#include <cstdlib>
#include <functional>
#include <gtest/gtest.h>
#include <iostream>
#include <queue>

TEST(Event, BasicTests) {

  NMXData eva(100, 200, 300);
  NMXData evb(101, 200, 300);

  ASSERT_EQ(100, eva.getdetector());
  ASSERT_EQ(200, eva.gettime());
  ASSERT_EQ(300, eva.getadc());

  ASSERT_EQ(true, eva < evb);
  ASSERT_EQ(false, evb < eva);
}

TEST(Event, AllocationTime) {
  srand(4242);
  std::vector<int> pqsizes{250000,  500000,  750000,  1000000, 1500000,
                           2000000, 3000000, 4000000, 5000000};

  for (auto const &size : pqsizes) {
    Timer tm;
    std::priority_queue<NMXData, std::vector<NMXData>, std::greater<NMXData>>
        pq;

    for (int i = 0; i < size; i++) {
      auto ev = new NMXData(random(), size, 1);
      pq.push(*ev);
    }
    for (int i = 0; i < size; i++) {
      pq.pop();
    }
    std::cout << size << " constructors " << tm.timeus() << " us\n";
  }
}
<commit_msg>reduced running time for unit test<commit_after>/** Copyright (C) 2016 European Spallation Source */

#include <libs/include/NMXEvent.h>
#include <libs/include/Timer.h>
#include <cstdlib>
#include <functional>
#include <gtest/gtest.h>
#include <iostream>
#include <queue>

TEST(Event, BasicTests) {

  NMXData eva(100, 200, 300);
  NMXData evb(101, 200, 300);

  ASSERT_EQ(100, eva.getdetector());
  ASSERT_EQ(200, eva.gettime());
  ASSERT_EQ(300, eva.getadc());

  ASSERT_EQ(true, eva < evb);
  ASSERT_EQ(false, evb < eva);
}

TEST(Event, AllocationTime) {
  srand(4242);
  //std::vector<int> pqsizes{250000,  500000,  750000,  1000000, 1500000,
  //                         2000000, 3000000, 4000000, 5000000};
  std::vector<int> pqsizes{250000,  500000}; // Jenkins Valgrind

  for (auto const &size : pqsizes) {
    Timer tm;
    std::priority_queue<NMXData, std::vector<NMXData>, std::greater<NMXData>>
        pq;

    for (int i = 0; i < size; i++) {
      auto ev = new NMXData(random(), size, 1);
      pq.push(*ev);
    }
    for (int i = 0; i < size; i++) {
      pq.pop();
    }
    std::cout << size << " constructors " << tm.timeus() << " us\n";
  }
}
<|endoftext|>