<commit_before>/*

Copyright (c) 2013, Project OSRM, Dennis Luxen, others
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 HOLDER 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 "OSRM.h"

OSRM::OSRM( const ServerPaths & server_paths, const bool use_shared_memory )
 :
    shm(
        boost::interprocess::open_or_create,
        "SharedBarriers",
        boost::interprocess::read_write
    ),
    use_shared_memory(use_shared_memory)
{
    if( !use_shared_memory ) {
        SimpleLogger().Write() << "loading data into internal memory";
        query_data_facade = new InternalDataFacade<QueryEdge::EdgeData>(
            server_paths
        );
    } else {
        region = boost::interprocess::mapped_region(
            shm,                             //What to map
            boost::interprocess::read_write  //Map it as read-write
        );
        shm.truncate( sizeof(SharedBarriers) );
        barrier = static_cast<SharedBarriers *>( region.get_address() );

        SimpleLogger().Write() << "loading data from shared memory";
        query_data_facade = new SharedDataFacade<QueryEdge::EdgeData>(
            server_paths
        );
    }

    //The following plugins handle all requests.
    RegisterPlugin(
        new HelloWorldPlugin()
    );
    RegisterPlugin(
        new LocatePlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
    RegisterPlugin(
        new NearestPlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
    RegisterPlugin(
        new TimestampPlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
    RegisterPlugin(
        new ViaRoutePlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
}

OSRM::~OSRM() {
    BOOST_FOREACH(PluginMap::value_type & plugin_pointer, plugin_map) {
        delete plugin_pointer.second;
    }
}

void OSRM::RegisterPlugin(BasePlugin * plugin) {
    SimpleLogger().Write()  << "loaded plugin: " << plugin->GetDescriptor();
    if( plugin_map.find(plugin->GetDescriptor()) != plugin_map.end() ) {
        delete plugin_map.find(plugin->GetDescriptor())->second;
    }
    plugin_map.emplace(plugin->GetDescriptor(), plugin);
}

void OSRM::RunQuery(RouteParameters & route_parameters, http::Reply & reply) {
    const PluginMap::const_iterator & iter = plugin_map.find(
        route_parameters.service
    );

    if(plugin_map.end() != iter) {
        reply.status = http::Reply::ok;
        if( use_shared_memory ) {
            //TODO lock update pending

            //TODO lock query

            //TODO unlock update pending

            //TODO ++query_count

            //TODO unlock query

            //wait until we get the mutex and free it immediately
            //TODO: increment semaphore of querying processes
            boost::interprocess::scoped_lock<
                boost::interprocess::interprocess_mutex
            > lock(barrier->update_mutex);
        }
        iter->second->HandleRequest(route_parameters, reply );
        if( use_shared_memory ) {
            //TODO unlock update pending
            //TODO --query_count
            //if (0 == query_count) {
                //TODO notify.all query_count 0
            //}
            //TODO unlock query

        }
    } else {
        reply = http::Reply::stockReply(http::Reply::badRequest);
    }
}
<commit_msg>implement shared lock in query<commit_after>/*

Copyright (c) 2013, Project OSRM, Dennis Luxen, others
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 HOLDER 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 "OSRM.h"

OSRM::OSRM( const ServerPaths & server_paths, const bool use_shared_memory )
 :
    shm(
        boost::interprocess::open_or_create,
        "SharedBarriers",
        boost::interprocess::read_write
    ),
    use_shared_memory(use_shared_memory)
{
    if( !use_shared_memory ) {
        SimpleLogger().Write() << "loading data into internal memory";
        query_data_facade = new InternalDataFacade<QueryEdge::EdgeData>(
            server_paths
        );
    } else {
        region = boost::interprocess::mapped_region(
            shm,                             //What to map
            boost::interprocess::read_write  //Map it as read-write
        );
        shm.truncate( sizeof(SharedBarriers) );
        barrier = static_cast<SharedBarriers *>( region.get_address() );

        SimpleLogger().Write() << "loading data from shared memory";
        query_data_facade = new SharedDataFacade<QueryEdge::EdgeData>(
            server_paths
        );
    }

    //The following plugins handle all requests.
    RegisterPlugin(
        new HelloWorldPlugin()
    );
    RegisterPlugin(
        new LocatePlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
    RegisterPlugin(
        new NearestPlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
    RegisterPlugin(
        new TimestampPlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
    RegisterPlugin(
        new ViaRoutePlugin<BaseDataFacade<QueryEdge::EdgeData> >(
            query_data_facade
        )
    );
}

OSRM::~OSRM() {
    BOOST_FOREACH(PluginMap::value_type & plugin_pointer, plugin_map) {
        delete plugin_pointer.second;
    }
}

void OSRM::RegisterPlugin(BasePlugin * plugin) {
    SimpleLogger().Write()  << "loaded plugin: " << plugin->GetDescriptor();
    if( plugin_map.find(plugin->GetDescriptor()) != plugin_map.end() ) {
        delete plugin_map.find(plugin->GetDescriptor())->second;
    }
    plugin_map.emplace(plugin->GetDescriptor(), plugin);
}

void OSRM::RunQuery(RouteParameters & route_parameters, http::Reply & reply) {
    const PluginMap::const_iterator & iter = plugin_map.find(
        route_parameters.service
    );

    if(plugin_map.end() != iter) {
        reply.status = http::Reply::ok;
        if( use_shared_memory ) {
            // lock update pending
            boost::interprocess::scoped_lock<
                boost::interprocess::named_mutex
            > pending_lock(barrier->pending_update_mutex);

            // lock query
            boost::interprocess::scoped_lock<
                boost::interprocess::named_mutex
            > query_lock(barrier->query_mutex);

            // unlock update pending
            pending_lock.unlock();

            // increment query count
            ++(barrier->number_of_queries);
        }
        iter->second->HandleRequest(route_parameters, reply );
        if( use_shared_memory ) {
            // lock query
            boost::interprocess::scoped_lock<
                boost::interprocess::named_mutex
            > query_lock(barrier->query_mutex);

            // decrement query count
            --(barrier->number_of_queries);
            BOOST_ASSERT_MSG(
                0 <= barrier->number_of_queries,
                "invalid number of queries"
            );

            if (0 == barrier->number_of_queries) {
                // notify all processes that were waiting for this condition
                barrier->no_running_queries_condition.notify_all();
            }
        }
    } else {
        reply = http::Reply::stockReply(http::Reply::badRequest);
    }
}
<|endoftext|>


<commit_before>/*
  c2ffi
  Copyright (C) 2013  Ryan Pavlik

  This file is part of c2ffi.

  c2ffi 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 2 of the License, or
  (at your option) any later version.

  c2ffi is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with c2ffi.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <map>

#include <llvm/Support/raw_ostream.h>
#include <llvm/Support/Host.h>
#include <llvm/ADT/IntrusiveRefCntPtr.h>

#include <clang/Basic/DiagnosticOptions.h>
#include <clang/Frontend/TextDiagnosticPrinter.h>
#include <clang/Frontend/CompilerInstance.h>
#include <clang/Basic/TargetOptions.h>
#include <clang/Basic/TargetInfo.h>
#include <clang/Basic/FileManager.h>
#include <clang/Basic/SourceManager.h>
#include <clang/Lex/HeaderSearch.h>
#include <clang/Lex/Preprocessor.h>
#include <clang/Basic/Diagnostic.h>
#include <clang/AST/ASTContext.h>
#include <clang/AST/ASTConsumer.h>
#include <clang/Parse/Parser.h>
#include <clang/Parse/ParseAST.h>

#include "c2ffi.h"
#include "c2ffi/ast.h"

using namespace c2ffi;

static std::string value_to_string(clang::APValue *v) {
    std::string s;
    llvm::raw_string_ostream ss(s);

    if(v->isInt() && v->getInt().isSigned())
        v->getInt().print(ss, true);
    else if(v->isInt())
        v->getInt().print(ss, false);
    else if(v->isFloat())
        ss << v->getFloat().convertToDouble();

    ss.flush();
    return s;
}

void C2FFIASTConsumer::HandleTopLevelDeclInObjCContainer(clang::DeclGroupRef d) {
    _od->write_comment("HandleTopLevelDeclInObjCContainer");
}

bool C2FFIASTConsumer::HandleTopLevelDecl(clang::DeclGroupRef d) {
    clang::DeclGroupRef::iterator it;

    for(it = d.begin(); it != d.end(); it++) {
        Decl *decl = NULL;

        if((*it)->isInvalidDecl()) {
            std::cerr << "Skipping invalid Decl:" << std::endl;
            (*it)->dump();
            continue;
        }

        if_cast(x, clang::VarDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::FunctionDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::RecordDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::EnumDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::TypedefDecl, *it) { decl = make_decl(x); }

        /* ObjC */
        else if_cast(x, clang::ObjCInterfaceDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::ObjCCategoryDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::ObjCProtocolDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::ObjCImplementationDecl, *it) continue;
        else if_cast(x, clang::ObjCMethodDecl, *it) continue;

        /* Always should be last */
        else if_cast(x, clang::NamedDecl, *it) { decl = make_decl(x); }
        else decl = make_decl(*it);

        if(decl) {
            decl->set_location(_ci, (*it));

            if(_mid) _od->write_between();
            else _mid = true;

            _od->write(*decl);
            delete decl;
        }
    }

    return true;
}

bool C2FFIASTConsumer::is_cur_decl(const clang::Decl *d) const {
    return _cur_decls.count(d);
}

Decl* C2FFIASTConsumer::make_decl(const clang::Decl *d, bool is_toplevel) {
    return new UnhandledDecl("", d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::NamedDecl *d, bool is_toplevel) {
    return new UnhandledDecl(d->getDeclName().getAsString(),
                             d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::FunctionDecl *d, bool is_toplevel) {
    _cur_decls.insert(d);

    const clang::Type *return_type = d->getReturnType().getTypePtr();
    FunctionDecl *fd = new FunctionDecl(d->getDeclName().getAsString(),
                                        Type::make_type(this, return_type),
                                        d->isVariadic());

    for(clang::FunctionDecl::param_const_iterator i = d->param_begin();
        i != d->param_end(); i++) {
        fd->add_field(this, *i);
    }

    return fd;
}

Decl* C2FFIASTConsumer::make_decl(const clang::VarDecl *d, bool is_toplevel) {
    clang::ASTContext &ctx = _ci.getASTContext();
    clang::APValue *v = NULL;
    std::string name = d->getDeclName().getAsString(),
                value = "";
    bool is_string = false;

    if(name.substr(0, 8) == "__c2ffi_")
        name = name.substr(8, std::string::npos);

    if(d->hasInit() && ((v = d->evaluateValue()) ||
                        (v = d->getEvaluatedValue()))) {
        if(v->isLValue()) {
            clang::APValue::LValueBase base = v->getLValueBase();
            const clang::Expr *e = base.get<const clang::Expr*>();

            if_const_cast(s, clang::StringLiteral, e) {
                value = s->getString();
                is_string = true;
            }
        } else {
            value = value_to_string(v);
        }
    }

    Type *t = Type::make_type(this, d->getTypeSourceInfo()->getType().getTypePtr());
    return new VarDecl(name, t, value, d->hasExternalStorage(), is_string);
}

Decl* C2FFIASTConsumer::make_decl(const clang::RecordDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();
    clang::ASTContext &ctx = _ci.getASTContext();
    const clang::Type *t = d->getTypeForDecl();

    if(is_toplevel && name == "") return NULL;

    _cur_decls.insert(d);
    RecordDecl *rd = new RecordDecl(name, d->isUnion());

    if(!t->isIncompleteType()) {
        rd->set_bit_size(ctx.getTypeSize(t));
        rd->set_bit_alignment(ctx.getTypeAlign(t));
    } else {
        rd->set_bit_size(0);
        rd->set_bit_alignment(0);
    }

    if(name == "") {
        _anon_decls[d] = _anon_id;
        rd->set_id(_anon_id);
        _anon_id++;
    }

    for(clang::RecordDecl::field_iterator i = d->field_begin();
        i != d->field_end(); i++)
        rd->add_field(this, *i);

    return rd;
}

bool is_underlying_valid(const clang::Type *t) {
    if_const_cast(e, clang::ElaboratedType, t) {
        return is_underlying_valid(e->getNamedType().getTypePtr());
    }

    if_const_cast(rt, clang::RecordType, t) {
        if(rt->getDecl()->isInvalidDecl())
            return false;
    }

    return true;
}

Decl* C2FFIASTConsumer::make_decl(const clang::TypedefDecl *d, bool is_toplevel) {
    const clang::Type *t = d->getTypeSourceInfo()->getType().getTypePtr();

    if(is_underlying_valid(t)) {
        return new TypedefDecl(d->getDeclName().getAsString(), Type::make_type(this, t));
    } else {
        std::cerr << "Skipping typedef to invalid type:" << std::endl;
        d->dump();
        return NULL;
    }
}

Decl* C2FFIASTConsumer::make_decl(const clang::EnumDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();

    _cur_decls.insert(d);
    EnumDecl *decl = new EnumDecl(name);

    if(name == "") {
        _anon_decls[d] = _anon_id;
        decl->set_id(_anon_id);
        _anon_id++;
    }

    for(clang::EnumDecl::enumerator_iterator i = d->enumerator_begin();
        i != d->enumerator_end(); i++) {
        const clang::EnumConstantDecl *ecd = (*i);
        decl->add_field(ecd->getDeclName().getAsString(),
                        ecd->getInitVal().getLimitedValue());
    }

    return decl;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCInterfaceDecl *d, bool is_toplevel) {
    const clang::ObjCInterfaceDecl *super = d->getSuperClass();

    _cur_decls.insert(d);
    ObjCInterfaceDecl *r = new ObjCInterfaceDecl(d->getDeclName().getAsString(),
                                                 super ? super->getDeclName().getAsString() : "",
                                                 !d->hasDefinition());

    for(clang::ObjCInterfaceDecl::protocol_iterator i = d->protocol_begin();
        i != d->protocol_end(); i++)
        r->add_protocol((*i)->getDeclName().getAsString());

    for(clang::ObjCInterfaceDecl::ivar_iterator i = d->ivar_begin();
        i != d->ivar_end(); i++) {
        r->add_field(this, *i);
    }

    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCCategoryDecl *d, bool is_toplevel) {
    ObjCCategoryDecl *r = new ObjCCategoryDecl(d->getClassInterface()->getDeclName().getAsString(),
                                               d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCProtocolDecl *d, bool is_toplevel) {
    ObjCProtocolDecl *r = new ObjCProtocolDecl(d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

unsigned int C2FFIASTConsumer::decl_id(const clang::Decl *d) const {
    ClangDeclIDMap::const_iterator it = _anon_decls.find(d);

    if(it != _anon_decls.end())
        return it->second;
    else
        return 0;
}
<commit_msg>Fix for typedefs with __attribute__ ((__mode__ ...))<commit_after>/*
  c2ffi
  Copyright (C) 2013  Ryan Pavlik

  This file is part of c2ffi.

  c2ffi 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 2 of the License, or
  (at your option) any later version.

  c2ffi is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with c2ffi.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <map>

#include <llvm/Support/raw_ostream.h>
#include <llvm/Support/Host.h>
#include <llvm/ADT/IntrusiveRefCntPtr.h>

#include <clang/Basic/DiagnosticOptions.h>
#include <clang/Frontend/TextDiagnosticPrinter.h>
#include <clang/Frontend/CompilerInstance.h>
#include <clang/Basic/TargetOptions.h>
#include <clang/Basic/TargetInfo.h>
#include <clang/Basic/FileManager.h>
#include <clang/Basic/SourceManager.h>
#include <clang/Lex/HeaderSearch.h>
#include <clang/Lex/Preprocessor.h>
#include <clang/Basic/Diagnostic.h>
#include <clang/AST/ASTContext.h>
#include <clang/AST/ASTConsumer.h>
#include <clang/Parse/Parser.h>
#include <clang/Parse/ParseAST.h>

#include "c2ffi.h"
#include "c2ffi/ast.h"

using namespace c2ffi;

static std::string value_to_string(clang::APValue *v) {
    std::string s;
    llvm::raw_string_ostream ss(s);

    if(v->isInt() && v->getInt().isSigned())
        v->getInt().print(ss, true);
    else if(v->isInt())
        v->getInt().print(ss, false);
    else if(v->isFloat())
        ss << v->getFloat().convertToDouble();

    ss.flush();
    return s;
}

void C2FFIASTConsumer::HandleTopLevelDeclInObjCContainer(clang::DeclGroupRef d) {
    _od->write_comment("HandleTopLevelDeclInObjCContainer");
}

bool C2FFIASTConsumer::HandleTopLevelDecl(clang::DeclGroupRef d) {
    clang::DeclGroupRef::iterator it;

    for(it = d.begin(); it != d.end(); it++) {
        Decl *decl = NULL;

        if((*it)->isInvalidDecl()) {
            std::cerr << "Skipping invalid Decl:" << std::endl;
            (*it)->dump();
            continue;
        }

        if_cast(x, clang::VarDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::FunctionDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::RecordDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::EnumDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::TypedefDecl, *it) { decl = make_decl(x); }

        /* ObjC */
        else if_cast(x, clang::ObjCInterfaceDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::ObjCCategoryDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::ObjCProtocolDecl, *it) { decl = make_decl(x); }
        else if_cast(x, clang::ObjCImplementationDecl, *it) continue;
        else if_cast(x, clang::ObjCMethodDecl, *it) continue;

        /* Always should be last */
        else if_cast(x, clang::NamedDecl, *it) { decl = make_decl(x); }
        else decl = make_decl(*it);

        if(decl) {
            decl->set_location(_ci, (*it));

            if(_mid) _od->write_between();
            else _mid = true;

            _od->write(*decl);
            delete decl;
        }
    }

    return true;
}

bool C2FFIASTConsumer::is_cur_decl(const clang::Decl *d) const {
    return _cur_decls.count(d);
}

Decl* C2FFIASTConsumer::make_decl(const clang::Decl *d, bool is_toplevel) {
    return new UnhandledDecl("", d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::NamedDecl *d, bool is_toplevel) {
    return new UnhandledDecl(d->getDeclName().getAsString(),
                             d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::FunctionDecl *d, bool is_toplevel) {
    _cur_decls.insert(d);

    const clang::Type *return_type = d->getReturnType().getTypePtr();
    FunctionDecl *fd = new FunctionDecl(d->getDeclName().getAsString(),
                                        Type::make_type(this, return_type),
                                        d->isVariadic());

    for(clang::FunctionDecl::param_const_iterator i = d->param_begin();
        i != d->param_end(); i++) {
        fd->add_field(this, *i);
    }

    return fd;
}

Decl* C2FFIASTConsumer::make_decl(const clang::VarDecl *d, bool is_toplevel) {
    clang::ASTContext &ctx = _ci.getASTContext();
    clang::APValue *v = NULL;
    std::string name = d->getDeclName().getAsString(),
                value = "";
    bool is_string = false;

    if(name.substr(0, 8) == "__c2ffi_")
        name = name.substr(8, std::string::npos);

    if(d->hasInit() && ((v = d->evaluateValue()) ||
                        (v = d->getEvaluatedValue()))) {
        if(v->isLValue()) {
            clang::APValue::LValueBase base = v->getLValueBase();
            const clang::Expr *e = base.get<const clang::Expr*>();

            if_const_cast(s, clang::StringLiteral, e) {
                value = s->getString();
                is_string = true;
            }
        } else {
            value = value_to_string(v);
        }
    }

    Type *t = Type::make_type(this, d->getTypeSourceInfo()->getType().getTypePtr());
    return new VarDecl(name, t, value, d->hasExternalStorage(), is_string);
}

Decl* C2FFIASTConsumer::make_decl(const clang::RecordDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();
    clang::ASTContext &ctx = _ci.getASTContext();
    const clang::Type *t = d->getTypeForDecl();

    if(is_toplevel && name == "") return NULL;

    _cur_decls.insert(d);
    RecordDecl *rd = new RecordDecl(name, d->isUnion());

    if(!t->isIncompleteType()) {
        rd->set_bit_size(ctx.getTypeSize(t));
        rd->set_bit_alignment(ctx.getTypeAlign(t));
    } else {
        rd->set_bit_size(0);
        rd->set_bit_alignment(0);
    }

    if(name == "") {
        _anon_decls[d] = _anon_id;
        rd->set_id(_anon_id);
        _anon_id++;
    }

    for(clang::RecordDecl::field_iterator i = d->field_begin();
        i != d->field_end(); i++)
        rd->add_field(this, *i);

    return rd;
}

bool is_underlying_valid(const clang::Type *t) {
    if_const_cast(e, clang::ElaboratedType, t) {
        return is_underlying_valid(e->getNamedType().getTypePtr());
    }

    if_const_cast(rt, clang::RecordType, t) {
        if(rt->getDecl()->isInvalidDecl())
            return false;
    }

    return true;
}

Decl* C2FFIASTConsumer::make_decl(const clang::TypedefDecl *d, bool is_toplevel) {
    const clang::Type *t = d->getUnderlyingType().getTypePtr();

    if(is_underlying_valid(t)) {
        return new TypedefDecl(d->getDeclName().getAsString(), Type::make_type(this, t));
    } else {
        std::cerr << "Skipping typedef to invalid type:" << std::endl;
        d->dump();
        return NULL;
    }
}

Decl* C2FFIASTConsumer::make_decl(const clang::EnumDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();

    _cur_decls.insert(d);
    EnumDecl *decl = new EnumDecl(name);

    if(name == "") {
        _anon_decls[d] = _anon_id;
        decl->set_id(_anon_id);
        _anon_id++;
    }

    for(clang::EnumDecl::enumerator_iterator i = d->enumerator_begin();
        i != d->enumerator_end(); i++) {
        const clang::EnumConstantDecl *ecd = (*i);
        decl->add_field(ecd->getDeclName().getAsString(),
                        ecd->getInitVal().getLimitedValue());
    }

    return decl;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCInterfaceDecl *d, bool is_toplevel) {
    const clang::ObjCInterfaceDecl *super = d->getSuperClass();

    _cur_decls.insert(d);
    ObjCInterfaceDecl *r = new ObjCInterfaceDecl(d->getDeclName().getAsString(),
                                                 super ? super->getDeclName().getAsString() : "",
                                                 !d->hasDefinition());

    for(clang::ObjCInterfaceDecl::protocol_iterator i = d->protocol_begin();
        i != d->protocol_end(); i++)
        r->add_protocol((*i)->getDeclName().getAsString());

    for(clang::ObjCInterfaceDecl::ivar_iterator i = d->ivar_begin();
        i != d->ivar_end(); i++) {
        r->add_field(this, *i);
    }

    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCCategoryDecl *d, bool is_toplevel) {
    ObjCCategoryDecl *r = new ObjCCategoryDecl(d->getClassInterface()->getDeclName().getAsString(),
                                               d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCProtocolDecl *d, bool is_toplevel) {
    ObjCProtocolDecl *r = new ObjCProtocolDecl(d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

unsigned int C2FFIASTConsumer::decl_id(const clang::Decl *d) const {
    ClangDeclIDMap::const_iterator it = _anon_decls.find(d);

    if(it != _anon_decls.end())
        return it->second;
    else
        return 0;
}
<|endoftext|>


<commit_before>// Copyright (c) 2016 Ansho Enigu
#include "crypto/lamport.h"

#include "crypto/ripemd160.h"
#include "crypto/common.h"
    bool LAMPORT::checksig(unsigned char* data, char sig[160][20], char pubkey[320][20])
    {
            bool messhashb[160];
            unsigned char* messhash;
            CRIPEMD160().Write(&data, data.size()).Finalize(&messhash);
            memcpy(messhashb, messhash, sizeof(messhashb));

            char _sig[160][20];
            char _csig[160][20];
            for(int i=0; i < 160; i++)
            {
              for(int o=0; o < 20; o++)
              {
                  if(messhashb[i])
                  {
                    _sig[i][o] = pubkey[2*i][o];
                  }
                  else
                  {
                    _sig[i][o] = pubkey[(2*i)+1][o];
                  }
              }
            }

            unsigned char* sighop;
            for(int i=0; i < 160; i++)
            {
              CRIPEMD160().Write(&_sig[i], _sig[i].size()).Finalize(&sighop);
              memcpy(_csig[i], sighop, _csig[i].size());
            }
            return sig == _csig;

    }

    char[320][20] LAMPORT::createsig(unsigned char* data, unsigned uint512_t prikey)
    {
      /* hash of the message */
      bool messhashb[160];
      unsigned char* messhash;
      CRIPEMD160().Write(&data, data.size()).Finalize(&messhash);

      /* creating true key from seed (the seed is used as the key by the user but it only is a form of compress key) */
      valtype vchHash(true ? 20 : 32);
      CRIPEMD160().Write(&prikey, prikey.size()).Finalize(&vchHash);
      for(int i =0; i < 320; i++)
      {
        unsigned char* tempHash;
        CRIPEMD160().Write(&vchHash, prikey.size()).Write(&i, i.size()).Finalize(&tempHash);
        prikeys[i] = temphash;
      }

      /* the signing will happen uder this */
      char sig[320][20];
      memcpy(messhashb, messhash, sizeof(messhashb));
      for(int i=0; i < 160; i++)
      {
        if(messhashb[i])
        {
          sig[i] = prikeys[2*i];
        }
        else
        {
          sig[i] = prikeys[(2*i)+1];
        }
      }
      return sig;
    }
<commit_msg>Update lamport.cpp<commit_after>// Copyright (c) 2016 Ansho Enigu
#include "crypto/lamport.h"

#include "crypto/ripemd160.h"
#include "crypto/common.h"
    bool LAMPORT::checksig(unsigned char* data, char sig[160][20], char pubkey[320][20])
    {
            bool messhashb[160];
            unsigned char* messhash;
            CRIPEMD160().Write(&data, data.size()).Finalize(&messhash);
            memcpy(messhashb, messhash, sizeof(messhashb));

            char _sig[160][20];
            char _csig[160][20];
            for(int i=0; i < 160; i++)
            {
              for(int o=0; o < 20; o++)
              {
                  if(messhashb[i])
                  {
                    _sig[i][o] = pubkey[2*i][o];
                  }
                  else
                  {
                    _sig[i][o] = pubkey[(2*i)+1][o];
                  }
              }
            }

            unsigned char* sighop;
            for(int i=0; i < 160; i++)
            {
              CRIPEMD160().Write(&_sig[i], _sig[i].size()).Finalize(&sighop);
              memcpy(_csig[i], sighop, _csig[i].size());
            }
            return sig == _csig;

    }

    char[320][20] LAMPORT::createsig(unsigned char* data, uint512_t prikey)
    {
      /* hash of the message */
      bool messhashb[160];
      unsigned char* messhash;
      CRIPEMD160().Write(&data, data.size()).Finalize(&messhash);

      /* creating true key from seed (the seed is used as the key by the user but it only is a form of compress key) */
      valtype vchHash(true ? 20 : 32);
      CRIPEMD160().Write(&prikey, prikey.size()).Finalize(&vchHash);
      for(int i =0; i < 320; i++)
      {
        unsigned char* tempHash;
        CRIPEMD160().Write(&vchHash, prikey.size()).Write(&i, i.size()).Finalize(&tempHash);
        prikeys[i] = temphash;
      }

      /* the signing will happen uder this */
      char sig[320][20];
      memcpy(messhashb, messhash, sizeof(messhashb));
      for(int i=0; i < 160; i++)
      {
        if(messhashb[i])
        {
          sig[i] = prikeys[2*i];
        }
        else
        {
          sig[i] = prikeys[(2*i)+1];
        }
      }
      return sig;
    }
<|endoftext|>


<commit_before>#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <errno.h>

#define MAX_BUF 1024

#include <sstream>
#include <iostream>
#include <string>
using namespace std;
#include <boost/property_tree/json_parser.hpp>

static int fd = -1;
#include <signal.h>
#include <stdio.h>
void sigcatch(int) {
	if(fd > 0) {
		close(fd);
	}
	exit(0);
}
int main()
{
	if (SIG_ERR == signal(SIGINT, sigcatch)) {
		printf("failed to set signal handler.n");
		exit(1);
	}
     fd = socket( AF_LOCAL, SOCK_DGRAM, 0 );

	struct sockaddr_un addr;
	bzero( &addr, sizeof(addr) );
	addr.sun_family = AF_LOCAL;
	strcpy( addr.sun_path, "/tmp/mpu.6050.unix.domain" );

   int use = 1;
   if (::setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &use, sizeof(int)) < 0) {
    perror("setsockopt(SO_REUSEADDR) failed");
   }
   if(::bind(fd, (struct sockaddr*)&addr, sizeof(addr)) < 0)
   {

     printf(" cannot bind socket: %d. \n",errno);
     close(fd);

     return 0;

   }

    char buf[MAX_BUF];
    int degree = 90;
    while(degree >0) {
    	socklen_t len = sizeof(addr);
        int ret = ::recvfrom(fd, buf, MAX_BUF, 0, (struct sockaddr *)&addr, &len);
        if(ret>0){
            //printf("Received: %s\n", buf);
            string recvStr(buf,ret);
            std::cout << recvStr << std::endl;
            std::stringstream ss;
            ss << recvStr;
            boost::property_tree::ptree pt;
            boost::property_tree::read_json(ss, pt);
            std::cout << pt.get<double>("yaw") << std::endl;
        } else if (ret==0){
        } else {
            printf("ret=<%d>\n",ret);
        }
    }
    close(fd);

    return 0;
}
<commit_msg>Update turn.cpp<commit_after>#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
#include <unistd.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <errno.h>

#define MAX_BUF 1024

#include <sstream>
#include <iostream>
#include <string>
using namespace std;
#include <boost/property_tree/json_parser.hpp>

static int fd = -1;
#include <signal.h>
#include <stdio.h>
void sigcatch(int) {
	if(fd > 0) {
		close(fd);
	}
	exit(0);
}
int main()
{
	if (SIG_ERR == signal(SIGINT, sigcatch)) {
		printf("failed to set signal handler.n");
		exit(1);
	}
     fd = socket( AF_LOCAL, SOCK_DGRAM, 0 );

	struct sockaddr_un addr;
	bzero( &addr, sizeof(addr) );
	addr.sun_family = AF_LOCAL;
	strcpy( addr.sun_path, "/tmp/mpu.6050.unix.domain" );

   int use = 1;
   if (::setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &use, sizeof(int)) < 0) {
    perror("setsockopt(SO_REUSEADDR) failed");
   }
   if(::bind(fd, (struct sockaddr*)&addr, sizeof(addr)) < 0)
   {

     printf(" cannot bind socket: %d. \n",errno);
     close(fd);

     return 0;

   }

    char buf[MAX_BUF];
    int degree = 90;
    while(degree >0) {
    	socklen_t len = sizeof(addr);
        int ret = ::recvfrom(fd, buf, MAX_BUF, 0, (struct sockaddr *)&addr, &len);
        if(ret>0){
            //printf("Received: %s\n", buf);
            string recvStr(buf,ret);
            std::cout << recvStr << std::endl;
            std::stringstream ss;
            ss << recvStr;
            try {
            	boost::property_tree::ptree pt;
            	boost::property_tree::read_json(ss, pt);
            	std::cout << pt.get<double>("yaw") << std::endl;
            } catch(std::exception e) {
            	std::cout << e.what() << std::endl;
            }
        } else if (ret==0){
        } else {
            printf("ret=<%d>\n",ret);
        }
    }
    close(fd);

    return 0;
}
<|endoftext|>


<commit_before>/*
  c2ffi
  Copyright (C) 2013  Ryan Pavlik

  This file is part of c2ffi.

  c2ffi 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 2 of the License, or
  (at your option) any later version.

  c2ffi is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with c2ffi.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <map>

#include <llvm/Support/raw_ostream.h>
#include <llvm/Support/Host.h>
#include <llvm/ADT/IntrusiveRefCntPtr.h>

#include <clang/Basic/DiagnosticOptions.h>
#include <clang/Frontend/TextDiagnosticPrinter.h>
#include <clang/Frontend/CompilerInstance.h>
#include <clang/Basic/TargetOptions.h>
#include <clang/Basic/TargetInfo.h>
#include <clang/Basic/FileManager.h>
#include <clang/Basic/SourceManager.h>
#include <clang/Lex/HeaderSearch.h>
#include <clang/Lex/Preprocessor.h>
#include <clang/Basic/Diagnostic.h>
#include <clang/AST/ASTContext.h>
#include <clang/AST/ASTConsumer.h>
#include <clang/AST/DeclTemplate.h>
#include <clang/AST/RecordLayout.h>
#include <clang/Parse/Parser.h>
#include <clang/Parse/ParseAST.h>

#include "c2ffi.h"
#include "c2ffi/ast.h"

using namespace c2ffi;

static std::string value_to_string(clang::APValue *v) {
    std::string s;
    llvm::raw_string_ostream ss(s);

    if(v->isInt() && v->getInt().isSigned())
        v->getInt().print(ss, true);
    else if(v->isInt())
        v->getInt().print(ss, false);
    else if(v->isFloat())
        ss << v->getFloat().convertToDouble();

    ss.flush();
    return s;
}

void C2FFIASTConsumer::HandleTopLevelDeclInObjCContainer(clang::DeclGroupRef d) {
    _od->write_comment("HandleTopLevelDeclInObjCContainer");
}

Decl* C2FFIASTConsumer::proc(const clang::Decl *d, Decl *decl) {
    if(!decl) return NULL;

    decl->set_ns(add_decl(_ns));

    if(decl->location() == "")
        decl->set_location(_ci, d);

    if(_mid) _od->write_between();
    else _mid = true;

    _od->write(*decl);
    return decl;
}

#define PROC decl = proc(d, make_decl(x))

void C2FFIASTConsumer::HandleDecl(clang::Decl *d, const clang::NamedDecl *ns) {
    Decl *decl = NULL;
    const clang::NamedDecl *old_ns = _ns;
    _ns = ns;

    if(d->isInvalidDecl()) {
        std::cerr << "Skipping invalid Decl:" << std::endl;
        d->dump();
        return;
    }

    /*
      std::cerr << "DECL:" << std::endl;
      d->dump();
    */

    if_cast(x, clang::NamespaceDecl, d) { PROC; HandleNS(x); }
    else if_cast(x, clang::VarDecl, d) PROC;

    /* C/C++ */
    else if_cast(x, clang::FieldDecl, d);
    else if_cast(x, clang::IndirectFieldDecl, d);
    else if_cast(x, clang::CXXMethodDecl, d);
    else if_cast(x, clang::FunctionTemplateDecl, d);
    else if_cast(x, clang::FunctionDecl, d) PROC;
    else if_cast(x, clang::CXXRecordDecl, d) { PROC; HandleDeclContext(x, x); }
    else if_cast(x, clang::RecordDecl, d) { PROC; HandleDeclContext(x, x); }
    else if_cast(x, clang::EnumDecl, d) PROC;
    else if_cast(x, clang::TypedefDecl, d) PROC;
    else if_cast(x, clang::ClassTemplateDecl, d);

    /* ObjC */
    else if_cast(x, clang::ObjCInterfaceDecl, d) PROC;
    else if_cast(x, clang::ObjCCategoryDecl, d) PROC;
    else if_cast(x, clang::ObjCProtocolDecl, d) PROC;
    else if_cast(x, clang::ObjCImplementationDecl, d);
    else if_cast(x, clang::ObjCMethodDecl, d);

    /* Always should be last */
    else if_cast(x, clang::NamedDecl, d) PROC;
    else decl = make_decl(d);

    if(decl) delete decl;
    _ns = old_ns;
}

void C2FFIASTConsumer::HandleNS(const clang::NamespaceDecl *ns) {
    HandleDeclContext(ns, ns);
}

void C2FFIASTConsumer::HandleDeclContext(const clang::DeclContext *dc,
                                         const clang::NamedDecl *ns) {
    clang::DeclContext::decl_iterator it;

    for(it = dc->decls_begin(); it != dc->decls_end(); ++it)
        HandleDecl(*it, ns);
}

bool C2FFIASTConsumer::HandleTopLevelDecl(clang::DeclGroupRef d) {
    clang::DeclGroupRef::iterator it;

    for(it = d.begin(); it != d.end(); ++it)
        HandleDecl(*it);

    return true;
}

void C2FFIASTConsumer::PostProcess() {
    if(!_config.template_output) return;

    std::ofstream &out = *_config.template_output;

    out << "#include \"" << _config.filename << "\"" << std::endl;

    for(ClangDeclSet::iterator i = _cxx_decls.begin(); i != _cxx_decls.end(); ++i) {
        const clang::Decl *d = (*i);

        if_const_cast(x, clang::ClassTemplateSpecializationDecl, d) {
            if(x->getSpecializationKind()) continue;
            if_const_cast(y, clang::ClassTemplatePartialSpecializationDecl, d)
                continue;

            write_template(x, out);
        }
    }
}

bool C2FFIASTConsumer::is_cur_decl(const clang::Decl *d) const {
    return _cur_decls.count(d);
}

Decl* C2FFIASTConsumer::make_decl(const clang::Decl *d, bool is_toplevel) {
    return new UnhandledDecl("", d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::NamedDecl *d, bool is_toplevel) {
    return new UnhandledDecl(d->getDeclName().getAsString(),
                             d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::FunctionDecl *d, bool is_toplevel) {
    _cur_decls.insert(d);

    clang::FunctionTemplateSpecializationInfo *spec =
        d->getTemplateSpecializationInfo();
    const clang::Type *return_type = d->getReturnType().getTypePtr();
    FunctionDecl *fd = new FunctionDecl(this,
                                        d->getDeclName().getAsString(),
                                        Type::make_type(this, return_type),
                                        d->isVariadic(),
                                        d->isInlineSpecified(),
                                        d->getStorageClass(),
                                        (spec ? spec->TemplateArguments : NULL));

    for(clang::FunctionDecl::param_const_iterator i = d->param_begin();
        i != d->param_end(); i++) {
        fd->add_field(this, *i);
    }

    return fd;
}

Decl* C2FFIASTConsumer::make_decl(const clang::VarDecl *d, bool is_toplevel) {
    clang::ASTContext &ctx = _ci.getASTContext();
    clang::APValue *v = NULL;
    std::string name = d->getDeclName().getAsString();
    std::string value = "";
    std::string loc = "";
    bool is_string = false;

    if(name.substr(0, 8) == "__c2ffi_") {
        name = name.substr(8, std::string::npos);

        clang::Preprocessor &pp = _ci.getPreprocessor();
        clang::IdentifierInfo &ii = pp.getIdentifierTable().get(llvm::StringRef(name));
        const clang::MacroInfo *mi = pp.getMacroInfo(&ii);

        if(mi)
            loc = mi->getDefinitionLoc().printToString(_ci.getSourceManager());
    }

    if(d->hasInit()) {
        if(!d->getType()->isDependentType()) {
            clang::EvaluatedStmt *stmt = d->ensureEvaluatedStmt();
            clang::Expr *e = clang::cast<clang::Expr>(stmt->Value);
            if(!e->isValueDependent() &&
               ((v = d->evaluateValue()) ||
                (v = d->getEvaluatedValue()))) {
                if(v->isLValue()) {
                    clang::APValue::LValueBase base = v->getLValueBase();
                    if(!base.isNull() && base.is<const clang::Expr*>()) {
                        const clang::Expr *e = base.get<const clang::Expr*>();

                        if_const_cast(s, clang::StringLiteral, e) {
                            value = s->getString();
                            is_string = true;
                        }
                    }
                } else {
                    value = value_to_string(v);
                }
            }
        }
    }

    Type *t = Type::make_type(this, d->getTypeSourceInfo()->getType().getTypePtr());
    VarDecl *cv = new VarDecl(name, t, value, d->hasExternalStorage(), is_string);

    if(loc != "")
        cv->set_location(loc);

    return cv;
}

Decl* C2FFIASTConsumer::make_decl(const clang::RecordDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();

    if(is_toplevel && name == "") return NULL;

    _cur_decls.insert(d);
    RecordDecl *rd = new RecordDecl(name, d->isUnion());
    rd->fill_record_decl(this, d);

    return rd;
}

bool is_underlying_valid(const clang::Type *t) {
    if_const_cast(e, clang::ElaboratedType, t) {
        return is_underlying_valid(e->getNamedType().getTypePtr());
    }

    if_const_cast(rt, clang::RecordType, t) {
        if(rt->getDecl()->isInvalidDecl())
            return false;
    }

    return true;
}

Decl* C2FFIASTConsumer::make_decl(const clang::TypedefDecl *d, bool is_toplevel) {
    const clang::Type *t = d->getUnderlyingType().getTypePtr();

    if(is_underlying_valid(t)) {
        return new TypedefDecl(d->getDeclName().getAsString(), Type::make_type(this, t));
    } else {
        std::cerr << "Skipping typedef to invalid type:" << std::endl;
        d->dump();
        return NULL;
    }
}

Decl* C2FFIASTConsumer::make_decl(const clang::EnumDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();

    _cur_decls.insert(d);
    EnumDecl *decl = new EnumDecl(name);

    if(name == "") {
        decl->set_id(add_decl(d));
    }

    for(clang::EnumDecl::enumerator_iterator i = d->enumerator_begin();
        i != d->enumerator_end(); ++i) {
        const clang::EnumConstantDecl *ecd = (*i);
        decl->add_field(ecd->getDeclName().getAsString(),
                        ecd->getInitVal().getLimitedValue());
    }

    return decl;
}

Decl* C2FFIASTConsumer::make_decl(const clang::CXXRecordDecl *d, bool is_toplevel) {
    if(!d->hasDefinition() || d->getDefinition()->isInvalidDecl())
        return NULL;

    std::string name = d->getDeclName().getAsString();
    const clang::TemplateArgumentList *template_args = NULL;

    if(is_toplevel && name == "") return NULL;

    if_const_cast(cts, clang::ClassTemplateSpecializationDecl, d) {
        template_args = &(cts->getTemplateArgs());
    }

    bool dependent = d->isDependentType();

    _cur_decls.insert(d);
    CXXRecordDecl *rd = new CXXRecordDecl(this, name, d->isUnion(), d->isClass(),
                                          template_args);
    rd->set_id(add_cxx_decl(d));
    rd->add_functions(this, d);

    if(!dependent) {
        rd->fill_record_decl(this, d);

        const clang::ASTRecordLayout &layout = _ci.getASTContext().getASTRecordLayout(d);

        for(clang::CXXRecordDecl::base_class_const_iterator i = d->bases_begin();
            i != d->bases_end(); ++i) {
            bool is_virtual = (*i).isVirtual();
            const clang::CXXRecordDecl *decl =
                (*i).getType().getTypePtr()->getAsCXXRecordDecl();
            int64_t offset = 0;

            if(is_virtual)
                offset = layout.getVBaseClassOffset(decl).getQuantity();
            else
                offset = layout.getBaseClassOffset(decl).getQuantity();

            rd->add_parent(decl->getNameAsString(),
                           (CXXRecordDecl::Access)(*i).getAccessSpecifier(),
                           offset, is_virtual);
        }
    }
    return rd;
}

Decl* C2FFIASTConsumer::make_decl(const clang::NamespaceDecl *d, bool is_toplevel) {
    CXXNamespaceDecl *ns = new CXXNamespaceDecl(d->getNameAsString());
    ns->set_id(add_cxx_decl(d));
    ns->set_ns(add_cxx_decl(_ns));

    return ns;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCInterfaceDecl *d, bool is_toplevel) {
    const clang::ObjCInterfaceDecl *super = d->getSuperClass();

    _cur_decls.insert(d);
    ObjCInterfaceDecl *r = new ObjCInterfaceDecl(d->getDeclName().getAsString(),
                                                 super ? super->getDeclName().getAsString() : "",
                                                 !d->hasDefinition());

    for(clang::ObjCInterfaceDecl::protocol_iterator i = d->protocol_begin();
        i != d->protocol_end(); i++)
        r->add_protocol((*i)->getDeclName().getAsString());

    for(clang::ObjCInterfaceDecl::ivar_iterator i = d->ivar_begin();
        i != d->ivar_end(); i++) {
        r->add_field(this, *i);
    }

    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCCategoryDecl *d, bool is_toplevel) {
    ObjCCategoryDecl *r = new ObjCCategoryDecl(d->getClassInterface()->getDeclName().getAsString(),
                                               d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCProtocolDecl *d, bool is_toplevel) {
    ObjCProtocolDecl *r = new ObjCProtocolDecl(d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

unsigned int C2FFIASTConsumer::decl_id(const clang::Decl *d) const {
    ClangDeclIDMap::const_iterator it = _decl_map.find(d);

    if(it != _decl_map.end())
        return it->second;
    else
        return 0;
}
<commit_msg>Quick hack to possibly work around #77, and stub the beginning of wchar_t etc support<commit_after>/*
  c2ffi
  Copyright (C) 2013  Ryan Pavlik

  This file is part of c2ffi.

  c2ffi 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 2 of the License, or
  (at your option) any later version.

  c2ffi is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with c2ffi.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <map>

#include <llvm/Support/raw_ostream.h>
#include <llvm/Support/Host.h>
#include <llvm/ADT/IntrusiveRefCntPtr.h>

#include <clang/Basic/DiagnosticOptions.h>
#include <clang/Frontend/TextDiagnosticPrinter.h>
#include <clang/Frontend/CompilerInstance.h>
#include <clang/Basic/TargetOptions.h>
#include <clang/Basic/TargetInfo.h>
#include <clang/Basic/FileManager.h>
#include <clang/Basic/SourceManager.h>
#include <clang/Lex/HeaderSearch.h>
#include <clang/Lex/Preprocessor.h>
#include <clang/Basic/Diagnostic.h>
#include <clang/AST/ASTContext.h>
#include <clang/AST/ASTConsumer.h>
#include <clang/AST/DeclTemplate.h>
#include <clang/AST/RecordLayout.h>
#include <clang/Parse/Parser.h>
#include <clang/Parse/ParseAST.h>

#include "c2ffi.h"
#include "c2ffi/ast.h"

using namespace c2ffi;

static std::string value_to_string(clang::APValue *v) {
    std::string s;
    llvm::raw_string_ostream ss(s);

    if(v->isInt() && v->getInt().isSigned())
        v->getInt().print(ss, true);
    else if(v->isInt())
        v->getInt().print(ss, false);
    else if(v->isFloat())
        ss << v->getFloat().convertToDouble();

    ss.flush();
    return s;
}

void C2FFIASTConsumer::HandleTopLevelDeclInObjCContainer(clang::DeclGroupRef d) {
    _od->write_comment("HandleTopLevelDeclInObjCContainer");
}

Decl* C2FFIASTConsumer::proc(const clang::Decl *d, Decl *decl) {
    if(!decl) return NULL;

    decl->set_ns(add_decl(_ns));

    if(decl->location() == "")
        decl->set_location(_ci, d);

    if(_mid) _od->write_between();
    else _mid = true;

    _od->write(*decl);
    return decl;
}

#define PROC decl = proc(d, make_decl(x))

void C2FFIASTConsumer::HandleDecl(clang::Decl *d, const clang::NamedDecl *ns) {
    Decl *decl = NULL;
    const clang::NamedDecl *old_ns = _ns;
    _ns = ns;

    if(d->isInvalidDecl()) {
        std::cerr << "Skipping invalid Decl:" << std::endl;
        d->dump();
        return;
    }

    /*
      std::cerr << "DECL:" << std::endl;
      d->dump();
    */

    if_cast(x, clang::NamespaceDecl, d) { PROC; HandleNS(x); }
    else if_cast(x, clang::VarDecl, d) PROC;

    /* C/C++ */
    else if_cast(x, clang::FieldDecl, d);
    else if_cast(x, clang::IndirectFieldDecl, d);
    else if_cast(x, clang::CXXMethodDecl, d);
    else if_cast(x, clang::FunctionTemplateDecl, d);
    else if_cast(x, clang::FunctionDecl, d) PROC;
    else if_cast(x, clang::CXXRecordDecl, d) { PROC; HandleDeclContext(x, x); }
    else if_cast(x, clang::RecordDecl, d) { PROC; HandleDeclContext(x, x); }
    else if_cast(x, clang::EnumDecl, d) PROC;
    else if_cast(x, clang::TypedefDecl, d) PROC;
    else if_cast(x, clang::ClassTemplateDecl, d);

    /* ObjC */
    else if_cast(x, clang::ObjCInterfaceDecl, d) PROC;
    else if_cast(x, clang::ObjCCategoryDecl, d) PROC;
    else if_cast(x, clang::ObjCProtocolDecl, d) PROC;
    else if_cast(x, clang::ObjCImplementationDecl, d);
    else if_cast(x, clang::ObjCMethodDecl, d);

    /* Always should be last */
    else if_cast(x, clang::NamedDecl, d) PROC;
    else decl = make_decl(d);

    if(decl) delete decl;
    _ns = old_ns;
}

void C2FFIASTConsumer::HandleNS(const clang::NamespaceDecl *ns) {
    HandleDeclContext(ns, ns);
}

void C2FFIASTConsumer::HandleDeclContext(const clang::DeclContext *dc,
                                         const clang::NamedDecl *ns) {
    clang::DeclContext::decl_iterator it;

    for(it = dc->decls_begin(); it != dc->decls_end(); ++it)
        HandleDecl(*it, ns);
}

bool C2FFIASTConsumer::HandleTopLevelDecl(clang::DeclGroupRef d) {
    clang::DeclGroupRef::iterator it;

    for(it = d.begin(); it != d.end(); ++it)
        HandleDecl(*it);

    return true;
}

void C2FFIASTConsumer::PostProcess() {
    if(!_config.template_output) return;

    std::ofstream &out = *_config.template_output;

    out << "#include \"" << _config.filename << "\"" << std::endl;

    for(ClangDeclSet::iterator i = _cxx_decls.begin(); i != _cxx_decls.end(); ++i) {
        const clang::Decl *d = (*i);

        if_const_cast(x, clang::ClassTemplateSpecializationDecl, d) {
            if(x->getSpecializationKind()) continue;
            if_const_cast(y, clang::ClassTemplatePartialSpecializationDecl, d)
                continue;

            write_template(x, out);
        }
    }
}

bool C2FFIASTConsumer::is_cur_decl(const clang::Decl *d) const {
    return _cur_decls.count(d);
}

Decl* C2FFIASTConsumer::make_decl(const clang::Decl *d, bool is_toplevel) {
    return new UnhandledDecl("", d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::NamedDecl *d, bool is_toplevel) {
    return new UnhandledDecl(d->getDeclName().getAsString(),
                             d->getDeclKindName());
}

Decl* C2FFIASTConsumer::make_decl(const clang::FunctionDecl *d, bool is_toplevel) {
    _cur_decls.insert(d);

    clang::FunctionTemplateSpecializationInfo *spec =
        d->getTemplateSpecializationInfo();
    const clang::Type *return_type = d->getReturnType().getTypePtr();
    FunctionDecl *fd = new FunctionDecl(this,
                                        d->getDeclName().getAsString(),
                                        Type::make_type(this, return_type),
                                        d->isVariadic(),
                                        d->isInlineSpecified(),
                                        d->getStorageClass(),
                                        (spec ? spec->TemplateArguments : NULL));

    for(clang::FunctionDecl::param_const_iterator i = d->param_begin();
        i != d->param_end(); i++) {
        fd->add_field(this, *i);
    }

    return fd;
}

Decl* C2FFIASTConsumer::make_decl(const clang::VarDecl *d, bool is_toplevel) {
    clang::ASTContext &ctx = _ci.getASTContext();
    clang::APValue *v = NULL;
    std::string name = d->getDeclName().getAsString();
    std::string value = "";
    std::string loc = "";
    bool is_string = false;

    if(name.substr(0, 8) == "__c2ffi_") {
        name = name.substr(8, std::string::npos);

        clang::Preprocessor &pp = _ci.getPreprocessor();
        clang::IdentifierInfo &ii = pp.getIdentifierTable().get(llvm::StringRef(name));
        const clang::MacroInfo *mi = pp.getMacroInfo(&ii);

        if(mi)
            loc = mi->getDefinitionLoc().printToString(_ci.getSourceManager());
    }

    if(d->hasInit()) {
        if(!d->getType()->isDependentType()) {
            clang::EvaluatedStmt *stmt = d->ensureEvaluatedStmt();
            clang::Expr *e = clang::cast<clang::Expr>(stmt->Value);
            if(!e->isValueDependent() &&
               ((v = d->evaluateValue()) ||
                (v = d->getEvaluatedValue()))) {
                if(v->isLValue()) {
                    clang::APValue::LValueBase base = v->getLValueBase();
                    if(!base.isNull() && base.is<const clang::Expr*>()) {
                        const clang::Expr *e = base.get<const clang::Expr*>();

                        if_const_cast(s, clang::StringLiteral, e) {
                            is_string = true;

                            if(s->isAscii() || s->isUTF8()) {
                                value = s->getString();
                            } else {

                            }
                        }
                    }
                } else {
                    value = value_to_string(v);
                }
            }
        }
    }

    Type *t = Type::make_type(this, d->getTypeSourceInfo()->getType().getTypePtr());
    VarDecl *cv = new VarDecl(name, t, value, d->hasExternalStorage(), is_string);

    if(loc != "")
        cv->set_location(loc);

    return cv;
}

Decl* C2FFIASTConsumer::make_decl(const clang::RecordDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();

    if(is_toplevel && name == "") return NULL;

    _cur_decls.insert(d);
    RecordDecl *rd = new RecordDecl(name, d->isUnion());
    rd->fill_record_decl(this, d);

    return rd;
}

bool is_underlying_valid(const clang::Type *t) {
    if_const_cast(e, clang::ElaboratedType, t) {
        return is_underlying_valid(e->getNamedType().getTypePtr());
    }

    if_const_cast(rt, clang::RecordType, t) {
        if(rt->getDecl()->isInvalidDecl())
            return false;
    }

    return true;
}

Decl* C2FFIASTConsumer::make_decl(const clang::TypedefDecl *d, bool is_toplevel) {
    const clang::Type *t = d->getUnderlyingType().getTypePtr();

    if(is_underlying_valid(t)) {
        return new TypedefDecl(d->getDeclName().getAsString(), Type::make_type(this, t));
    } else {
        std::cerr << "Skipping typedef to invalid type:" << std::endl;
        d->dump();
        return NULL;
    }
}

Decl* C2FFIASTConsumer::make_decl(const clang::EnumDecl *d, bool is_toplevel) {
    std::string name = d->getDeclName().getAsString();

    _cur_decls.insert(d);
    EnumDecl *decl = new EnumDecl(name);

    if(name == "") {
        decl->set_id(add_decl(d));
    }

    for(clang::EnumDecl::enumerator_iterator i = d->enumerator_begin();
        i != d->enumerator_end(); ++i) {
        const clang::EnumConstantDecl *ecd = (*i);
        decl->add_field(ecd->getDeclName().getAsString(),
                        ecd->getInitVal().getLimitedValue());
    }

    return decl;
}

Decl* C2FFIASTConsumer::make_decl(const clang::CXXRecordDecl *d, bool is_toplevel) {
    if(!d->hasDefinition() || d->getDefinition()->isInvalidDecl())
        return NULL;

    std::string name = d->getDeclName().getAsString();
    const clang::TemplateArgumentList *template_args = NULL;

    if(is_toplevel && name == "") return NULL;

    if_const_cast(cts, clang::ClassTemplateSpecializationDecl, d) {
        template_args = &(cts->getTemplateArgs());
    }

    bool dependent = d->isDependentType();

    _cur_decls.insert(d);
    CXXRecordDecl *rd = new CXXRecordDecl(this, name, d->isUnion(), d->isClass(),
                                          template_args);
    rd->set_id(add_cxx_decl(d));
    rd->add_functions(this, d);

    if(!dependent) {
        rd->fill_record_decl(this, d);

        const clang::ASTRecordLayout &layout = _ci.getASTContext().getASTRecordLayout(d);

        for(clang::CXXRecordDecl::base_class_const_iterator i = d->bases_begin();
            i != d->bases_end(); ++i) {
            bool is_virtual = (*i).isVirtual();
            const clang::CXXRecordDecl *decl =
                (*i).getType().getTypePtr()->getAsCXXRecordDecl();
            int64_t offset = 0;

            if(is_virtual)
                offset = layout.getVBaseClassOffset(decl).getQuantity();
            else
                offset = layout.getBaseClassOffset(decl).getQuantity();

            rd->add_parent(decl->getNameAsString(),
                           (CXXRecordDecl::Access)(*i).getAccessSpecifier(),
                           offset, is_virtual);
        }
    }
    return rd;
}

Decl* C2FFIASTConsumer::make_decl(const clang::NamespaceDecl *d, bool is_toplevel) {
    CXXNamespaceDecl *ns = new CXXNamespaceDecl(d->getNameAsString());
    ns->set_id(add_cxx_decl(d));
    ns->set_ns(add_cxx_decl(_ns));

    return ns;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCInterfaceDecl *d, bool is_toplevel) {
    const clang::ObjCInterfaceDecl *super = d->getSuperClass();

    _cur_decls.insert(d);
    ObjCInterfaceDecl *r = new ObjCInterfaceDecl(d->getDeclName().getAsString(),
                                                 super ? super->getDeclName().getAsString() : "",
                                                 !d->hasDefinition());

    for(clang::ObjCInterfaceDecl::protocol_iterator i = d->protocol_begin();
        i != d->protocol_end(); i++)
        r->add_protocol((*i)->getDeclName().getAsString());

    for(clang::ObjCInterfaceDecl::ivar_iterator i = d->ivar_begin();
        i != d->ivar_end(); i++) {
        r->add_field(this, *i);
    }

    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCCategoryDecl *d, bool is_toplevel) {
    ObjCCategoryDecl *r = new ObjCCategoryDecl(d->getClassInterface()->getDeclName().getAsString(),
                                               d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

Decl* C2FFIASTConsumer::make_decl(const clang::ObjCProtocolDecl *d, bool is_toplevel) {
    ObjCProtocolDecl *r = new ObjCProtocolDecl(d->getDeclName().getAsString());
    _cur_decls.insert(d);
    r->add_functions(this, d);
    return r;
}

unsigned int C2FFIASTConsumer::decl_id(const clang::Decl *d) const {
    ClangDeclIDMap::const_iterator it = _decl_map.find(d);

    if(it != _decl_map.end())
        return it->second;
    else
        return 0;
}
<|endoftext|>


<commit_before>/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "nav-precomp.h" // Precomp header

#include <mrpt/nav/reactive/CWaypointsNavigator.h>
#include <mrpt/poses/CPose2D.h>

using namespace mrpt::nav;
using namespace std;

CWaypointsNavigator::CWaypointsNavigator(CRobot2NavInterface &robot_if) :
	CAbstractNavigator(robot_if),
	MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT(-1.0),
	MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS(1)
{
}

CWaypointsNavigator::~CWaypointsNavigator()
{
}

void CWaypointsNavigator::navigateWaypoints( const TWaypointSequence & nav_request )
{
	MRPT_START

	mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

	m_waypoint_nav_status = TWaypointStatusSequence();
	m_waypoint_nav_status.timestamp_nav_started = mrpt::system::now();

	const size_t N = nav_request.waypoints.size();
	ASSERTMSG_(N>0,"List of waypoints is empty!");

	m_waypoint_nav_status.waypoints.resize(N);
	// Copy waypoints fields data, leave status fields to defaults:
	for (size_t i=0;i<N;i++)
	{
		ASSERT_( nav_request.waypoints[i].isValid() );
		m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
	}

	m_waypoint_nav_status.waypoint_index_current_goal = -1;  // Not started yet.

	// The main loop navigationStep() will iterate over waypoints and send them to navigate()
	MRPT_END
}

void CWaypointsNavigator::getWaypointNavStatus(TWaypointStatusSequence & out_nav_status) const
{
	// No need to lock mutex...
	out_nav_status = m_waypoint_nav_status;
}

void CWaypointsNavigator::cancel()
{
	{
		mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);
		m_waypoint_nav_status = TWaypointStatusSequence();
	}
	CAbstractNavigator::cancel();
}

void CWaypointsNavigator::navigationStep()
{
	MRPT_START

	using mrpt::utils::square;

	// Call base navigation step to execute one-single waypoint navigation, as usual:
	CAbstractNavigator::navigationStep();  // This internally locks "m_nav_cs"

	// --------------------------------------
	//     Waypoint navigation algorithm
	// --------------------------------------
	{
		mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

		TWaypointStatusSequence &wps = m_waypoint_nav_status; // shortcut to save typing

		if ( wps.waypoints.empty() || wps.final_goal_reached )
			return; // No nav request is pending or it was canceled

		// 0) Get current robot pose:
		CAbstractNavigator::updateCurrentPoseAndSpeeds();

		// 1) default policy: go thru WPs one by one
		const int prev_wp_index = wps.waypoint_index_current_goal;

		mrpt::math::TSegment2D robot_move_seg;
		robot_move_seg.point1.x = m_curPoseVel.pose.x;
		robot_move_seg.point1.y = m_curPoseVel.pose.y;
		if (wps.last_robot_pose.x==TWaypoint::INVALID_NUM)
		{
			robot_move_seg.point2 = robot_move_seg.point1;
		}
		else
		{
			robot_move_seg.point2.x = wps.last_robot_pose.x;
			robot_move_seg.point2.y = wps.last_robot_pose.y;
		}
		wps.last_robot_pose = m_curPoseVel.pose; // save for next iters

		if (wps.waypoint_index_current_goal>=0 &&
			robot_move_seg.distance( wps.waypoints[wps.waypoint_index_current_goal].target ) < wps.waypoints[wps.waypoint_index_current_goal].allowed_distance )
		{
			wps.waypoints[wps.waypoint_index_current_goal].reached = true;
			m_robot.sendWaypointReachedEvent(wps.waypoint_index_current_goal);

			// Was this the final goal??
			if ( wps.waypoint_index_current_goal < int(wps.waypoints.size()-1) ) {
				wps.waypoint_index_current_goal++;
			}
			else {
				wps.final_goal_reached = true;
			}
		}

		// 2) More advanced policy: if available, use children class methods to decide
		//     which is the best candidate for the next waypoint, if we can skip current one:
		if (!wps.final_goal_reached && wps.waypoint_index_current_goal >= 0)
		{
			const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
			int most_advanced_wp = wps.waypoint_index_current_goal;
			const int most_advanced_wp_at_begin = most_advanced_wp;

			for (int idx=wps.waypoint_index_current_goal;idx<(int)wps.waypoints.size();idx++)
			{
				if (idx<0) continue;

				// Is it reachable?
				mrpt::math::TPoint2D wp_local_wrt_robot;
				robot_pose.inverseComposePoint(wps.waypoints[idx].target, wp_local_wrt_robot);

				if (MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT>0 && wp_local_wrt_robot.norm()>MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT)
					continue; // Skip this one, it is too far away

				const bool is_reachable = this->impl_waypoint_is_reachable(wp_local_wrt_robot);

				if (is_reachable) {
					// Robustness filter: only skip to a future waypoint if it is seen as "reachable" during
					// a given number of timesteps:
					if (++wps.waypoints[idx].counter_seen_reachable > MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS) {
						most_advanced_wp = idx;
					}
				}

				// Is allowed to skip it?
				if (!wps.waypoints[idx].allow_skip)
					break; // Do not keep trying, since we are now allowed to skip this one.
			}

			if (most_advanced_wp>=0) {
				wps.waypoint_index_current_goal = most_advanced_wp;
				for (int k=most_advanced_wp_at_begin;k<most_advanced_wp;k++) {
					wps.waypoints[k].reached = true;
					m_robot.sendWaypointReachedEvent(k);
				}
			}
		}

		// Still not started and no better guess? Start with the first waypoint:
		if (wps.waypoint_index_current_goal<0)
			wps.waypoint_index_current_goal = 0;

		// 3) Should I request a new (single target) navigation command?
		//    Only if the temporary goal changed:
		if (wps.waypoint_index_current_goal>=0 && prev_wp_index!=wps.waypoint_index_current_goal)
		{
			ASSERT_( wps.waypoint_index_current_goal < int(wps.waypoints.size()) );
			TWaypointStatus &wp = wps.waypoints[wps.waypoint_index_current_goal];
			const bool is_final_wp =  ( (wps.waypoint_index_current_goal+1)==int(wps.waypoints.size()) );

			m_robot.sendNewWaypointTargetEvent(wps.waypoint_index_current_goal);

			CAbstractNavigator::TNavigationParams nav_cmd;
			nav_cmd.target.x = wp.target.x;
			nav_cmd.target.y = wp.target.y;
			nav_cmd.target.phi = (wp.target_heading!=TWaypoint::INVALID_NUM ? wp.target_heading : .0);
			nav_cmd.targetAllowedDistance = wp.allowed_distance;
			nav_cmd.targetIsRelative = false;
			nav_cmd.targetIsIntermediaryWaypoint = !is_final_wp;

			this->navigate( &nav_cmd );
		}
	}

	MRPT_END
}

void CWaypointsNavigator::loadWaypointsParamsConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sectionName)
{
	MRPT_LOAD_CONFIG_VAR(MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT,  double,   cfg, sectionName);
	MRPT_LOAD_CONFIG_VAR(MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS, int, cfg, sectionName);
}

void CWaypointsNavigator::onStartNewNavigation()
{

}

bool CWaypointsNavigator::isRelativePointReachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const
{
	return impl_waypoint_is_reachable(wp_local_wrt_robot);
}
<commit_msg>fix order of end-of-nav and waypoint-reached events<commit_after>/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "nav-precomp.h" // Precomp header

#include <mrpt/nav/reactive/CWaypointsNavigator.h>
#include <mrpt/poses/CPose2D.h>

using namespace mrpt::nav;
using namespace std;

CWaypointsNavigator::CWaypointsNavigator(CRobot2NavInterface &robot_if) :
	CAbstractNavigator(robot_if),
	MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT(-1.0),
	MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS(1)
{
}

CWaypointsNavigator::~CWaypointsNavigator()
{
}

void CWaypointsNavigator::navigateWaypoints( const TWaypointSequence & nav_request )
{
	MRPT_START

	mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

	m_waypoint_nav_status = TWaypointStatusSequence();
	m_waypoint_nav_status.timestamp_nav_started = mrpt::system::now();

	const size_t N = nav_request.waypoints.size();
	ASSERTMSG_(N>0,"List of waypoints is empty!");

	m_waypoint_nav_status.waypoints.resize(N);
	// Copy waypoints fields data, leave status fields to defaults:
	for (size_t i=0;i<N;i++)
	{
		ASSERT_( nav_request.waypoints[i].isValid() );
		m_waypoint_nav_status.waypoints[i] = nav_request.waypoints[i];
	}

	m_waypoint_nav_status.waypoint_index_current_goal = -1;  // Not started yet.

	// The main loop navigationStep() will iterate over waypoints and send them to navigate()
	MRPT_END
}

void CWaypointsNavigator::getWaypointNavStatus(TWaypointStatusSequence & out_nav_status) const
{
	// No need to lock mutex...
	out_nav_status = m_waypoint_nav_status;
}

void CWaypointsNavigator::cancel()
{
	{
		mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);
		m_waypoint_nav_status = TWaypointStatusSequence();
	}
	CAbstractNavigator::cancel();
}

void CWaypointsNavigator::navigationStep()
{
	MRPT_START

	using mrpt::utils::square;

	// --------------------------------------
	//     Waypoint navigation algorithm
	// --------------------------------------
	{
		mrpt::synch::CCriticalSectionLocker csl(&m_nav_waypoints_cs);

		TWaypointStatusSequence &wps = m_waypoint_nav_status; // shortcut to save typing

		if ( wps.waypoints.empty() || wps.final_goal_reached )
			return; // No nav request is pending or it was canceled

		// 0) Get current robot pose:
		CAbstractNavigator::updateCurrentPoseAndSpeeds();

		// 1) default policy: go thru WPs one by one
		const int prev_wp_index = wps.waypoint_index_current_goal;

		mrpt::math::TSegment2D robot_move_seg;
		robot_move_seg.point1.x = m_curPoseVel.pose.x;
		robot_move_seg.point1.y = m_curPoseVel.pose.y;
		if (wps.last_robot_pose.x==TWaypoint::INVALID_NUM)
		{
			robot_move_seg.point2 = robot_move_seg.point1;
		}
		else
		{
			robot_move_seg.point2.x = wps.last_robot_pose.x;
			robot_move_seg.point2.y = wps.last_robot_pose.y;
		}
		wps.last_robot_pose = m_curPoseVel.pose; // save for next iters

		if (wps.waypoint_index_current_goal>=0 &&
			robot_move_seg.distance( wps.waypoints[wps.waypoint_index_current_goal].target ) < wps.waypoints[wps.waypoint_index_current_goal].allowed_distance )
		{
			wps.waypoints[wps.waypoint_index_current_goal].reached = true;
			m_robot.sendWaypointReachedEvent(wps.waypoint_index_current_goal);

			// Was this the final goal??
			if ( wps.waypoint_index_current_goal < int(wps.waypoints.size()-1) ) {
				wps.waypoint_index_current_goal++;
			}
			else {
				wps.final_goal_reached = true;
			}
		}

		// 2) More advanced policy: if available, use children class methods to decide
		//     which is the best candidate for the next waypoint, if we can skip current one:
		if (!wps.final_goal_reached && wps.waypoint_index_current_goal >= 0)
		{
			const mrpt::poses::CPose2D robot_pose(m_curPoseVel.pose);
			int most_advanced_wp = wps.waypoint_index_current_goal;
			const int most_advanced_wp_at_begin = most_advanced_wp;

			for (int idx=wps.waypoint_index_current_goal;idx<(int)wps.waypoints.size();idx++)
			{
				if (idx<0) continue;

				// Is it reachable?
				mrpt::math::TPoint2D wp_local_wrt_robot;
				robot_pose.inverseComposePoint(wps.waypoints[idx].target, wp_local_wrt_robot);

				if (MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT>0 && wp_local_wrt_robot.norm()>MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT)
					continue; // Skip this one, it is too far away

				const bool is_reachable = this->impl_waypoint_is_reachable(wp_local_wrt_robot);

				if (is_reachable) {
					// Robustness filter: only skip to a future waypoint if it is seen as "reachable" during
					// a given number of timesteps:
					if (++wps.waypoints[idx].counter_seen_reachable > MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS) {
						most_advanced_wp = idx;
					}
				}

				// Is allowed to skip it?
				if (!wps.waypoints[idx].allow_skip)
					break; // Do not keep trying, since we are now allowed to skip this one.
			}

			if (most_advanced_wp>=0) {
				wps.waypoint_index_current_goal = most_advanced_wp;
				for (int k=most_advanced_wp_at_begin;k<most_advanced_wp;k++) {
					wps.waypoints[k].reached = true;
					m_robot.sendWaypointReachedEvent(k);
				}
			}
		}

		// Still not started and no better guess? Start with the first waypoint:
		if (wps.waypoint_index_current_goal<0)
			wps.waypoint_index_current_goal = 0;

		// 3) Should I request a new (single target) navigation command?
		//    Only if the temporary goal changed:
		if (wps.waypoint_index_current_goal>=0 && prev_wp_index!=wps.waypoint_index_current_goal)
		{
			ASSERT_( wps.waypoint_index_current_goal < int(wps.waypoints.size()) );
			TWaypointStatus &wp = wps.waypoints[wps.waypoint_index_current_goal];
			const bool is_final_wp =  ( (wps.waypoint_index_current_goal+1)==int(wps.waypoints.size()) );

			m_robot.sendNewWaypointTargetEvent(wps.waypoint_index_current_goal);

			CAbstractNavigator::TNavigationParams nav_cmd;
			nav_cmd.target.x = wp.target.x;
			nav_cmd.target.y = wp.target.y;
			nav_cmd.target.phi = (wp.target_heading!=TWaypoint::INVALID_NUM ? wp.target_heading : .0);
			nav_cmd.targetAllowedDistance = wp.allowed_distance;
			nav_cmd.targetIsRelative = false;
			nav_cmd.targetIsIntermediaryWaypoint = !is_final_wp;

			this->navigate( &nav_cmd );
		}
	}

	// Note: navigationStep() called *after* waypoints part to get end-of-navigation events *after*
	//       waypoints-related events:

	// Call base navigation step to execute one-single waypoint navigation, as usual:
	CAbstractNavigator::navigationStep();  // This internally locks "m_nav_cs"

	MRPT_END
}

void CWaypointsNavigator::loadWaypointsParamsConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sectionName)
{
	MRPT_LOAD_CONFIG_VAR(MAX_DISTANCE_TO_ALLOW_SKIP_WAYPOINT,  double,   cfg, sectionName);
	MRPT_LOAD_CONFIG_VAR(MIN_TIMESTEPS_CONFIRM_SKIP_WAYPOINTS, int, cfg, sectionName);
}

void CWaypointsNavigator::onStartNewNavigation()
{

}

bool CWaypointsNavigator::isRelativePointReachable(const mrpt::math::TPoint2D &wp_local_wrt_robot) const
{
	return impl_waypoint_is_reachable(wp_local_wrt_robot);
}
<|endoftext|>


<commit_before>/*
  Copyright (c) 2015 Michael C. Miller. All right reserved.

  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.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  */

#if defined(ESP8266)

#include <Arduino.h>
#include <Servo.h>

#define INVALID_SERVO         255     // flag indicating an invalid servo index

const uint32_t c_CycleCompensation = 4;  // compensation us to trim adjust for digitalWrite delays

struct ServoInfo  {
    uint8_t pin : 6;             // a pin number from 0 to 63
    uint8_t isActive : 1;        // true if this channel is enabled, pin not pulsed if false
};

struct ServoState {
    ServoInfo info;
    volatile uint16_t usPulse;
};

#if !defined (SERVO_EXCLUDE_TIMER0)
ServoTimer0 s_servoTimer0;
#endif

#if !defined (SERVO_EXCLUDE_TIMER1)
ServoTimer1 s_servoTimer1;
#endif

static ServoState s_servos[MAX_SERVOS];     // static array of servo structures

static uint8_t s_servoCount = 0;            // the total number of attached s_servos

// inconvenience macros
#define SERVO_INDEX_TO_TIMER(servoIndex) ((ServoTimerSequence)(servoIndex / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO_INDEX(timerId, channel)  ((timerId * SERVOS_PER_TIMER) + channel)     // macro to access servo index by timer and channel

//------------------------------------------------------------------------------
// Interrupt handler template method that takes a class that implements
// a standard set of methods for the timer abstraction
//------------------------------------------------------------------------------
template <class T> void Servo_Handler(T* timer)
{
    uint8_t servoIndex;

    // clear interrupt
    timer->ResetInterrupt();

    if (timer->isEndOfCycle()) {
        timer->StartCycle();
    }
    else {
        servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel());
        if (servoIndex < s_servoCount && s_servos[servoIndex].info.isActive) {
            // pulse this channel low if activated
            digitalWrite(s_servos[servoIndex].info.pin, LOW);
        }
        timer->nextChannel();
    }

    servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel());

    if (servoIndex < s_servoCount && timer->getCurrentChannel() < SERVOS_PER_TIMER) {
        timer->SetPulseCompare(timer->usToTicks(s_servos[servoIndex].usPulse) - c_CycleCompensation);

        if (s_servos[servoIndex].info.isActive) {  // check if activated
            digitalWrite(s_servos[servoIndex].info.pin, HIGH); // its an active channel so pulse it high
        }
    }
    else {
        // finished all channels so wait for the refresh period to expire before starting over
        // allow a few ticks to ensure the next match is not missed
        uint32_t refreshCompare = timer->usToTicks(REFRESH_INTERVAL);
        if ((timer->GetCycleCount() + c_CycleCompensation * 2) < refreshCompare) {
            timer->SetCycleCompare(refreshCompare - c_CycleCompensation);
        }
        else {
            // at least REFRESH_INTERVAL has elapsed
            timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2);
        }

        timer->setEndOfCycle();
    }
}

static void initISR(ServoTimerSequence timerId)
{
#if !defined (SERVO_EXCLUDE_TIMER0)
    if (timerId == ServoTimerSequence_Timer0)
        s_servoTimer0.InitInterrupt([]() {Servo_Handler<ServoTimer0>(&s_servoTimer0); });
#endif
#if !defined (SERVO_EXCLUDE_TIMER1)
    if (timerId == ServoTimerSequence_Timer1)
        s_servoTimer1.InitInterrupt([]() {Servo_Handler<ServoTimer1>(&s_servoTimer1); });
#endif
}

static void finISR(ServoTimerSequence timerId)
{
#if !defined (SERVO_EXCLUDE_TIMER0)
    if (timerId == ServoTimerSequence_Timer0)
        s_servoTimer0.StopInterrupt();
#endif
#if !defined (SERVO_EXCLUDE_TIMER1)
    if (timerId == ServoTimerSequence_Timer1)
        s_servoTimer1.StopInterrupt();
#endif
}

// returns true if any servo is active on this timer
static boolean isTimerActive(ServoTimerSequence timerId)
{
    for (uint8_t channel = 0; channel < SERVOS_PER_TIMER; channel++) {
        if (s_servos[SERVO_INDEX(timerId, channel)].info.isActive) {
            return true;
        }
    }
    return false;
}

//-------------------------------------------------------------------
// Servo class methods

Servo::Servo()
{
    if (s_servoCount < MAX_SERVOS) {
        // assign a servo index to this instance
        _servoIndex = s_servoCount++;
        // store default values
        s_servos[_servoIndex].usPulse = DEFAULT_PULSE_WIDTH;
    }
    else {
        _servoIndex = INVALID_SERVO;  // too many servos
    }
}

uint8_t Servo::attach(int pin)
{
    return attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}

uint8_t Servo::attach(int pin, int minUs, int maxUs)
{
    ServoTimerSequence timerId;

    if (_servoIndex < MAX_SERVOS) {
        pinMode(pin, OUTPUT);       // set servo pin to output
        digitalWrite(pin, LOW);
        s_servos[_servoIndex].info.pin = pin;

        // keep the min and max within 200-3000 us, these are extreme
        // ranges and should support extreme servos while maintaining
        // reasonable ranges
        _maxUs = max(250, min(3000, maxUs));
        _minUs = max(200, min(_maxUs, minUs));

        // initialize the timerId if it has not already been initialized
        timerId = SERVO_INDEX_TO_TIMER(_servoIndex);
        if (!isTimerActive(timerId)) {
            initISR(timerId);
        }
        s_servos[_servoIndex].info.isActive = true;  // this must be set after the check for isTimerActive
    }
    return _servoIndex;
}

void Servo::detach()
{
    ServoTimerSequence timerId;

    s_servos[_servoIndex].info.isActive = false;
    timerId = SERVO_INDEX_TO_TIMER(_servoIndex);
    if (!isTimerActive(timerId)) {
        finISR(timerId);
    }
}

void Servo::write(int value)
{
    // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if (value < MIN_PULSE_WIDTH) {
        // assumed to be 0-180 degrees servo
        value = max(0, min(180, value));
        value = map(value, 0, 180, _minUs, _maxUs);
    }
    writeMicroseconds(value);
}

void Servo::writeMicroseconds(int value)
{
    // ensure channel is valid
    if ((_servoIndex < MAX_SERVOS)) {
        // ensure pulse width is valid
        value = max(_minUs, min(_maxUs, value));

        s_servos[_servoIndex].usPulse = value;
    }
}

int Servo::read() // return the value as degrees
{
    return map(readMicroseconds(), _minUs, _maxUs, 0, 180);
}

int Servo::readMicroseconds()
{
    unsigned int pulsewidth;
    if (_servoIndex != INVALID_SERVO) {
        pulsewidth = s_servos[_servoIndex].usPulse;
    }
    else {
        pulsewidth = 0;
    }

    return pulsewidth;
}

bool Servo::attached()
{
    return s_servos[_servoIndex].info.isActive;
}

#endif

<commit_msg>Fix to Servo allowing write() to be called before attach()<commit_after>/*
  Copyright (c) 2015 Michael C. Miller. All right reserved.

  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.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  */

#if defined(ESP8266)

#include <Arduino.h>
#include <Servo.h>

#define INVALID_SERVO         255     // flag indicating an invalid servo index

const uint32_t c_CycleCompensation = 4;  // compensation us to trim adjust for digitalWrite delays

struct ServoInfo  {
    uint8_t pin : 6;             // a pin number from 0 to 63
    uint8_t isActive : 1;        // true if this channel is enabled, pin not pulsed if false
};

struct ServoState {
    ServoInfo info;
    volatile uint16_t usPulse;
};

#if !defined (SERVO_EXCLUDE_TIMER0)
ServoTimer0 s_servoTimer0;
#endif

#if !defined (SERVO_EXCLUDE_TIMER1)
ServoTimer1 s_servoTimer1;
#endif

static ServoState s_servos[MAX_SERVOS];     // static array of servo structures

static uint8_t s_servoCount = 0;            // the total number of attached s_servos

// inconvenience macros
#define SERVO_INDEX_TO_TIMER(servoIndex) ((ServoTimerSequence)(servoIndex / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO_INDEX(timerId, channel)  ((timerId * SERVOS_PER_TIMER) + channel)     // macro to access servo index by timer and channel

//------------------------------------------------------------------------------
// Interrupt handler template method that takes a class that implements
// a standard set of methods for the timer abstraction
//------------------------------------------------------------------------------
template <class T> void Servo_Handler(T* timer)
{
    uint8_t servoIndex;

    // clear interrupt
    timer->ResetInterrupt();

    if (timer->isEndOfCycle()) {
        timer->StartCycle();
    }
    else {
        servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel());
        if (servoIndex < s_servoCount && s_servos[servoIndex].info.isActive) {
            // pulse this channel low if activated
            digitalWrite(s_servos[servoIndex].info.pin, LOW);
        }
        timer->nextChannel();
    }

    servoIndex = SERVO_INDEX(timer->timerId(), timer->getCurrentChannel());

    if (servoIndex < s_servoCount && timer->getCurrentChannel() < SERVOS_PER_TIMER) {
        timer->SetPulseCompare(timer->usToTicks(s_servos[servoIndex].usPulse) - c_CycleCompensation);

        if (s_servos[servoIndex].info.isActive) {  // check if activated
            digitalWrite(s_servos[servoIndex].info.pin, HIGH); // its an active channel so pulse it high
        }
    }
    else {
        // finished all channels so wait for the refresh period to expire before starting over
        // allow a few ticks to ensure the next match is not missed
        uint32_t refreshCompare = timer->usToTicks(REFRESH_INTERVAL);
        if ((timer->GetCycleCount() + c_CycleCompensation * 2) < refreshCompare) {
            timer->SetCycleCompare(refreshCompare - c_CycleCompensation);
        }
        else {
            // at least REFRESH_INTERVAL has elapsed
            timer->SetCycleCompare(timer->GetCycleCount() + c_CycleCompensation * 2);
        }

        timer->setEndOfCycle();
    }
}

static void initISR(ServoTimerSequence timerId)
{
#if !defined (SERVO_EXCLUDE_TIMER0)
    if (timerId == ServoTimerSequence_Timer0)
        s_servoTimer0.InitInterrupt([]() {Servo_Handler<ServoTimer0>(&s_servoTimer0); });
#endif
#if !defined (SERVO_EXCLUDE_TIMER1)
    if (timerId == ServoTimerSequence_Timer1)
        s_servoTimer1.InitInterrupt([]() {Servo_Handler<ServoTimer1>(&s_servoTimer1); });
#endif
}

static void finISR(ServoTimerSequence timerId)
{
#if !defined (SERVO_EXCLUDE_TIMER0)
    if (timerId == ServoTimerSequence_Timer0)
        s_servoTimer0.StopInterrupt();
#endif
#if !defined (SERVO_EXCLUDE_TIMER1)
    if (timerId == ServoTimerSequence_Timer1)
        s_servoTimer1.StopInterrupt();
#endif
}

// returns true if any servo is active on this timer
static boolean isTimerActive(ServoTimerSequence timerId)
{
    for (uint8_t channel = 0; channel < SERVOS_PER_TIMER; channel++) {
        if (s_servos[SERVO_INDEX(timerId, channel)].info.isActive) {
            return true;
        }
    }
    return false;
}

//-------------------------------------------------------------------
// Servo class methods

Servo::Servo()
{
    if (s_servoCount < MAX_SERVOS) {
        // assign a servo index to this instance
        _servoIndex = s_servoCount++;
        // store default values
        s_servos[_servoIndex].usPulse = DEFAULT_PULSE_WIDTH;

        // set default _minUs and _maxUs incase write() is called before attach()
        _minUs = MIN_PULSE_WIDTH;
        _maxUs = MAX_PULSE_WIDTH;
    }
    else {
        _servoIndex = INVALID_SERVO;  // too many servos
    }
}

uint8_t Servo::attach(int pin)
{
    return attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
}

uint8_t Servo::attach(int pin, int minUs, int maxUs)
{
    ServoTimerSequence timerId;

    if (_servoIndex < MAX_SERVOS) {
        pinMode(pin, OUTPUT);       // set servo pin to output
        digitalWrite(pin, LOW);
        s_servos[_servoIndex].info.pin = pin;

        // keep the min and max within 200-3000 us, these are extreme
        // ranges and should support extreme servos while maintaining
        // reasonable ranges
        _maxUs = max(250, min(3000, maxUs));
        _minUs = max(200, min(_maxUs, minUs));

        // initialize the timerId if it has not already been initialized
        timerId = SERVO_INDEX_TO_TIMER(_servoIndex);
        if (!isTimerActive(timerId)) {
            initISR(timerId);
        }
        s_servos[_servoIndex].info.isActive = true;  // this must be set after the check for isTimerActive
    }
    return _servoIndex;
}

void Servo::detach()
{
    ServoTimerSequence timerId;

    s_servos[_servoIndex].info.isActive = false;
    timerId = SERVO_INDEX_TO_TIMER(_servoIndex);
    if (!isTimerActive(timerId)) {
        finISR(timerId);
    }
}

void Servo::write(int value)
{
    // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
    if (value < MIN_PULSE_WIDTH) {
        // assumed to be 0-180 degrees servo
        value = max(0, min(180, value));
        value = map(value, 0, 180, _minUs, _maxUs);
    }
    writeMicroseconds(value);
}

void Servo::writeMicroseconds(int value)
{
    // ensure channel is valid
    if ((_servoIndex < MAX_SERVOS)) {
        // ensure pulse width is valid
        value = max(_minUs, min(_maxUs, value));

        s_servos[_servoIndex].usPulse = value;
    }
}

int Servo::read() // return the value as degrees
{
    return map(readMicroseconds(), _minUs, _maxUs, 0, 180);
}

int Servo::readMicroseconds()
{
    unsigned int pulsewidth;
    if (_servoIndex != INVALID_SERVO) {
        pulsewidth = s_servos[_servoIndex].usPulse;
    }
    else {
        pulsewidth = 0;
    }

    return pulsewidth;
}

bool Servo::attached()
{
    return s_servos[_servoIndex].info.isActive;
}

#endif

<|endoftext|>


<commit_before>/* Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 * See cxx source for full Copyright notice                               */

/* $Id$ */

/// By Laurent Aphecetche

{
  cout << "Loading MUON libraries ..." << endl;
  gROOT->LoadMacro("${ALICE_ROOT}/MUON/loadlibs.C");
  gInterpreter->ProcessLine("loadlibs()");

  cout << "Setting include path ..." << endl;
  gSystem->SetIncludePath("-I${ALICE_ROOT}/include -I${ALICE_ROOT}/RAW -I${ALICE_ROOT}/MUON -I${ALICE_ROOT}/MUON/mapping");
}
<commit_msg>Extended include path to cover all directories needed by all macros.<commit_after>/* Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 * See cxx source for full Copyright notice                               */

/* $Id$ */

/// By Laurent Aphecetche

{
  cout << "Loading MUON libraries ..." << endl;
  gROOT->LoadMacro("${ALICE_ROOT}/MUON/loadlibs.C");
  gInterpreter->ProcessLine("loadlibs()");

  cout << "Setting include path ..." << endl;
  TString includePath = "-I${ALICE_ROOT}/include ";
  includePath        += "-I${ALICE_ROOT}/RAW ";
  includePath        += "-I${ALICE_ROOT}/FASTSIM ";
  includePath        += "-I${ALICE_ROOT}/EVGEN ";
  includePath        += "-I${ALICE_ROOT}/SHUTTLE/TestShuttle ";
  includePath        += "-I${ALICE_ROOT}/ITS ";
  includePath        += "-I${ALICE_ROOT}/MUON ";
  includePath        += "-I${ALICE_ROOT}/MUON/mapping";
  gSystem->SetIncludePath(includePath.Data());
}
<|endoftext|>