<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 "DataStructures/QueryEdge.h"
#include "DataStructures/SharedMemoryFactory.h"
#include "DataStructures/SharedMemoryVectorWrapper.h"
#include "DataStructures/StaticGraph.h"
#include "DataStructures/StaticRTree.h"
#include "Server/DataStructures/BaseDataFacade.h"
#include "Server/DataStructures/SharedDataType.h"
#include "Server/DataStructures/SharedBarriers.h"
#include "Util/BoostFileSystemFix.h"
#include "Util/ProgramOptions.h"
#include "Util/SimpleLogger.h"
#include "Util/UUID.h"
#include "typedefs.h"

#include <boost/integer.hpp>
#include <boost/filesystem/fstream.hpp>

#include <string>
#include <vector>

int main( const int argc, const char * argv[] ) {
    SharedBarriers barrier;

    boost::interprocess::scoped_lock<
        boost::interprocess::named_mutex
    > pending_lock(barrier.pending_update_mutex);

    try {
        LogPolicy::GetInstance().Unmute();
        SimpleLogger().Write(logDEBUG) << "Checking input parameters";

        bool use_shared_memory = false;
        std::string ip_address;
        int ip_port, requested_num_threads;

        ServerPaths server_paths;
        if(
            !GenerateServerProgramOptions(
                argc,
                argv,
                server_paths,
                ip_address,
                ip_port,
                requested_num_threads,
                use_shared_memory
            )
        ) {
            return 0;
        }
        if( server_paths.find("hsgrdata") == server_paths.end() ) {
            throw OSRMException("no hsgr file given in ini file");
        }
        if( server_paths.find("ramindex") == server_paths.end() ) {
            throw OSRMException("no ram index file given in ini file");
        }
        if( server_paths.find("fileindex") == server_paths.end() ) {
            throw OSRMException("no leaf index file given in ini file");
        }
        if( server_paths.find("nodesdata") == server_paths.end() ) {
            throw OSRMException("no nodes file given in ini file");
        }
        if( server_paths.find("edgesdata") == server_paths.end() ) {
            throw OSRMException("no edges file given in ini file");
        }
        if( server_paths.find("namesdata") == server_paths.end() ) {
            throw OSRMException("no names file given in ini file");
        }

        ServerPaths::const_iterator paths_iterator = server_paths.find("hsgrdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & hsgr_path = paths_iterator->second;
        paths_iterator = server_paths.find("timestamp");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & timestamp_path = paths_iterator->second;
        paths_iterator = server_paths.find("ramindex");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & ram_index_path = paths_iterator->second;
        paths_iterator = server_paths.find("fileindex");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const std::string & file_index_file_name = paths_iterator->second.string();
        paths_iterator = server_paths.find("nodesdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & nodes_data_path = paths_iterator->second;
        paths_iterator = server_paths.find("edgesdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & edges_data_path = paths_iterator->second;
        paths_iterator = server_paths.find("namesdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & names_data_path = paths_iterator->second;

        // get the shared memory segment to use
        bool use_first_segment = SharedMemory::RegionExists( LAYOUT_2 );
        SharedDataType LAYOUT = ( use_first_segment ? LAYOUT_1 : LAYOUT_2 );
        SharedDataType DATA   = ( use_first_segment ? DATA_1 : DATA_2 );

        // Allocate a memory layout in shared memory, deallocate previous
        SharedMemory * layout_memory = SharedMemoryFactory::Get(
            LAYOUT,
            sizeof(SharedDataLayout)
        );
        SharedDataLayout * shared_layout_ptr = static_cast<SharedDataLayout *>(
            layout_memory->Ptr()
        );
        shared_layout_ptr = new(layout_memory->Ptr()) SharedDataLayout();

        std::copy(
            file_index_file_name.begin(),
            (file_index_file_name.length() <= 1024 ? file_index_file_name.end() : file_index_file_name.begin()+1023),
            shared_layout_ptr->ram_index_file_name
        );
        // add zero termination
        unsigned end_of_string_index = std::min(1023ul, file_index_file_name.length());
        shared_layout_ptr->ram_index_file_name[end_of_string_index] = '\0';

        // collect number of elements to store in shared memory object
        SimpleLogger().Write(logDEBUG) << "Collecting files sizes";
        // number of entries in name index
        boost::filesystem::ifstream name_stream(
            names_data_path, std::ios::binary
        );
        unsigned name_index_size = 0;
        name_stream.read((char *)&name_index_size, sizeof(unsigned));
        shared_layout_ptr->name_index_list_size = name_index_size;
        // SimpleLogger().Write() << "name index size: " << shared_layout_ptr->name_index_list_size;
        BOOST_ASSERT_MSG(0 != shared_layout_ptr->name_index_list_size, "name file broken");

        unsigned number_of_chars = 0;
        name_stream.read((char *)&number_of_chars, sizeof(unsigned));
        shared_layout_ptr->name_char_list_size = number_of_chars;
        // SimpleLogger().Write() << "name char size: " << shared_layout_ptr->name_char_list_size;

        //Loading information for original edges
        boost::filesystem::ifstream edges_input_stream(
            edges_data_path,
            std::ios::binary
        );
        unsigned number_of_original_edges = 0;
        edges_input_stream.read((char*)&number_of_original_edges, sizeof(unsigned));

        shared_layout_ptr->via_node_list_size = number_of_original_edges;
        shared_layout_ptr->name_id_list_size = number_of_original_edges;
        shared_layout_ptr->turn_instruction_list_size = number_of_original_edges;

        boost::filesystem::ifstream hsgr_input_stream(
            hsgr_path,
            std::ios::binary
        );

        UUID uuid_loaded, uuid_orig;
        hsgr_input_stream.read((char *)&uuid_loaded, sizeof(UUID));
        if( !uuid_loaded.TestGraphUtil(uuid_orig) ) {
            SimpleLogger().Write(logWARNING) <<
                ".hsgr was prepared with different build. "
                "Reprocess to get rid of this warning.";
        } else {
            SimpleLogger().Write() << "UUID checked out ok";
        }

        // load checksum
        unsigned checksum = 0;
        hsgr_input_stream.read((char*)&checksum, sizeof(unsigned) );
        shared_layout_ptr->checksum = checksum;
        // load graph node size
        unsigned number_of_graph_nodes = 0;
        hsgr_input_stream.read(
            (char*) &number_of_graph_nodes,
            sizeof(unsigned)
        );

        BOOST_ASSERT_MSG(
            (0 != number_of_graph_nodes),
            "number of nodes is zero"
        );
        shared_layout_ptr->graph_node_list_size = number_of_graph_nodes;

        // load graph edge size
        unsigned number_of_graph_edges = 0;
        hsgr_input_stream.read( (char*) &number_of_graph_edges, sizeof(unsigned) );
        BOOST_ASSERT_MSG(
            0 != number_of_graph_edges,
            "number of graph edges is zero"
        );
        shared_layout_ptr->graph_edge_list_size = number_of_graph_edges;

        // load rsearch tree size
        boost::filesystem::ifstream tree_node_file(
            ram_index_path,
            std::ios::binary
        );

        uint32_t tree_size = 0;
        tree_node_file.read((char*)&tree_size, sizeof(uint32_t));
        shared_layout_ptr->r_search_tree_size = tree_size;

        //load timestamp size
        std::string m_timestamp;
        if( boost::filesystem::exists(timestamp_path) ) {
            boost::filesystem::ifstream timestampInStream( timestamp_path );
            if(!timestampInStream) {
                SimpleLogger().Write(logWARNING) <<
                    timestamp_path << " not found. setting to default";
            } else {
                getline(timestampInStream, m_timestamp);
                timestampInStream.close();
            }
        }
        if(m_timestamp.empty()) {
            m_timestamp = "n/a";
        }
        if(25 < m_timestamp.length()) {
            m_timestamp.resize(25);
        }
        shared_layout_ptr->timestamp_length = m_timestamp.length();

        //load coordinate size
        SimpleLogger().Write() <<
            "Loading coordinates list from " << nodes_data_path.string();
        boost::filesystem::ifstream nodes_input_stream(
            nodes_data_path,
            std::ios::binary
        );
        unsigned coordinate_list_size = 0;
        nodes_input_stream.read((char *)&coordinate_list_size, sizeof(unsigned));
        shared_layout_ptr->coordinate_list_size = coordinate_list_size;

        // allocate shared memory block

        SimpleLogger().Write() << "allocating shared memory of " << shared_layout_ptr->GetSizeOfLayout() << " bytes";
        SharedMemory * shared_memory = SharedMemoryFactory::Get(
            DATA,
            shared_layout_ptr->GetSizeOfLayout()
        );
        char * shared_memory_ptr = static_cast<char *>(shared_memory->Ptr());

        // read actual data into shared memory object //
        // Loading street names
        // SimpleLogger().Write() << "Loading names index and chars from: " << names_data_path.string();
        unsigned * name_index_ptr = (unsigned*)(
            shared_memory_ptr + shared_layout_ptr->GetNameIndexOffset()
        );
        // SimpleLogger().Write(logDEBUG) << "Bytes: " << shared_layout_ptr->name_index_list_size*sizeof(unsigned);

        name_stream.read(
            (char*)name_index_ptr,
            shared_layout_ptr->name_index_list_size*sizeof(unsigned)
        );

        char * name_char_ptr = shared_memory_ptr + shared_layout_ptr->GetNameListOffset();
        name_stream.read(
            name_char_ptr,
            shared_layout_ptr->name_char_list_size*sizeof(char)
        );
        name_stream.close();

        //load original edge information
        SimpleLogger().Write() <<
            "Loading via node, coordinates and turn instruction lists from: " <<
            edges_data_path.string();

        NodeID * via_node_ptr = (NodeID *)(
            shared_memory_ptr + shared_layout_ptr->GetViaNodeListOffset()
        );

        unsigned * name_id_ptr = (unsigned *)(
            shared_memory_ptr + shared_layout_ptr->GetNameIDListOffset()
        );

        TurnInstruction * turn_instructions_ptr = (TurnInstruction *)(
            shared_memory_ptr + shared_layout_ptr->GetTurnInstructionListOffset()
        );

        OriginalEdgeData current_edge_data;
        for(unsigned i = 0; i < number_of_original_edges; ++i) {
            // SimpleLogger().Write() << i << "/" << number_of_edges;
            edges_input_stream.read(
                (char*)&(current_edge_data),
                sizeof(OriginalEdgeData)
            );
            via_node_ptr[i] = current_edge_data.viaNode;
            name_id_ptr[i]  = current_edge_data.nameID;
            turn_instructions_ptr[i] = current_edge_data.turnInstruction;
        }
        edges_input_stream.close();

        // Loading list of coordinates
        FixedPointCoordinate * coordinates_ptr = (FixedPointCoordinate *)(
            shared_memory_ptr + shared_layout_ptr->GetCoordinateListOffset()
        );

        NodeInfo current_node;
        for(unsigned i = 0; i < coordinate_list_size; ++i) {
            nodes_input_stream.read((char *)&current_node, sizeof(NodeInfo));
            coordinates_ptr[i] = FixedPointCoordinate(current_node.lat, current_node.lon);
        }
        nodes_input_stream.close();

        //store timestamp
        char * timestamp_ptr = static_cast<char *>(
            shared_memory_ptr + shared_layout_ptr->GetTimeStampOffset()
        );
        std::copy(
            m_timestamp.c_str(),
            m_timestamp.c_str()+m_timestamp.length(),
            timestamp_ptr
        );

        // store search tree portion of rtree
        char * rtree_ptr = static_cast<char *>(
            shared_memory_ptr + shared_layout_ptr->GetRSearchTreeOffset()
        );

        tree_node_file.read(rtree_ptr, sizeof(RTreeNode)*tree_size);
        tree_node_file.close();

        // load the nodes of the search graph
        QueryGraph::_StrNode * graph_node_list_ptr = (QueryGraph::_StrNode*)(
            shared_memory_ptr + shared_layout_ptr->GetGraphNodeListOffset()
        );
        hsgr_input_stream.read(
            (char*) graph_node_list_ptr,
            shared_layout_ptr->graph_node_list_size*sizeof(QueryGraph::_StrNode)
        );

        // load the edges of the search graph
        QueryGraph::_StrEdge * graph_edge_list_ptr = (QueryGraph::_StrEdge *)(
            shared_memory_ptr + shared_layout_ptr->GetGraphEdgeListOffsett()
        );
        hsgr_input_stream.read(
            (char*) graph_edge_list_ptr,
            shared_layout_ptr->graph_edge_list_size*sizeof(QueryGraph::_StrEdge)
        );
        hsgr_input_stream.close();

        //TODO acquire lock
        SharedMemory * data_type_memory = SharedMemoryFactory::Get(
            CURRENT_REGIONS,
            2*sizeof(SharedDataType),
            true,
            false
        );
        SharedDataType * data_type_ptr = static_cast<SharedDataType *>(
            data_type_memory->Ptr()
        );

        data_type_ptr[0] = LAYOUT;
        data_type_ptr[1] = DATA;
        if(use_first_segment) {
            BOOST_ASSERT( DATA   == DATA_1   );
            BOOST_ASSERT( LAYOUT == LAYOUT_1 );
            SharedMemory::Remove(DATA_2);
            SharedMemory::Remove(LAYOUT_2);
        } else {
            BOOST_ASSERT(   DATA == DATA_2   );
            BOOST_ASSERT( LAYOUT == LAYOUT_2 );
            SharedMemory::Remove(DATA_1);
            SharedMemory::Remove(LAYOUT_1);
        }
        SimpleLogger().Write() << "all data loaded";
    } catch(const std::exception & e) {
        SimpleLogger().Write(logWARNING) << "caught exception: " << e.what();
    }

    return 0;
}
<commit_msg>make release build less verbose<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 "DataStructures/QueryEdge.h"
#include "DataStructures/SharedMemoryFactory.h"
#include "DataStructures/SharedMemoryVectorWrapper.h"
#include "DataStructures/StaticGraph.h"
#include "DataStructures/StaticRTree.h"
#include "Server/DataStructures/BaseDataFacade.h"
#include "Server/DataStructures/SharedDataType.h"
#include "Server/DataStructures/SharedBarriers.h"
#include "Util/BoostFileSystemFix.h"
#include "Util/ProgramOptions.h"
#include "Util/SimpleLogger.h"
#include "Util/UUID.h"
#include "typedefs.h"

#include <boost/integer.hpp>
#include <boost/filesystem/fstream.hpp>

#include <string>
#include <vector>

int main( const int argc, const char * argv[] ) {
    SharedBarriers barrier;

    boost::interprocess::scoped_lock<
        boost::interprocess::named_mutex
    > pending_lock(barrier.pending_update_mutex);

    try {
        LogPolicy::GetInstance().Unmute();
        SimpleLogger().Write(logDEBUG) << "Checking input parameters";

        bool use_shared_memory = false;
        std::string ip_address;
        int ip_port, requested_num_threads;

        ServerPaths server_paths;
        if(
            !GenerateServerProgramOptions(
                argc,
                argv,
                server_paths,
                ip_address,
                ip_port,
                requested_num_threads,
                use_shared_memory
            )
        ) {
            return 0;
        }
        if( server_paths.find("hsgrdata") == server_paths.end() ) {
            throw OSRMException("no hsgr file given in ini file");
        }
        if( server_paths.find("ramindex") == server_paths.end() ) {
            throw OSRMException("no ram index file given in ini file");
        }
        if( server_paths.find("fileindex") == server_paths.end() ) {
            throw OSRMException("no leaf index file given in ini file");
        }
        if( server_paths.find("nodesdata") == server_paths.end() ) {
            throw OSRMException("no nodes file given in ini file");
        }
        if( server_paths.find("edgesdata") == server_paths.end() ) {
            throw OSRMException("no edges file given in ini file");
        }
        if( server_paths.find("namesdata") == server_paths.end() ) {
            throw OSRMException("no names file given in ini file");
        }

        ServerPaths::const_iterator paths_iterator = server_paths.find("hsgrdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & hsgr_path = paths_iterator->second;
        paths_iterator = server_paths.find("timestamp");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & timestamp_path = paths_iterator->second;
        paths_iterator = server_paths.find("ramindex");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & ram_index_path = paths_iterator->second;
        paths_iterator = server_paths.find("fileindex");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const std::string & file_index_file_name = paths_iterator->second.string();
        paths_iterator = server_paths.find("nodesdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & nodes_data_path = paths_iterator->second;
        paths_iterator = server_paths.find("edgesdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & edges_data_path = paths_iterator->second;
        paths_iterator = server_paths.find("namesdata");
        BOOST_ASSERT(server_paths.end() != paths_iterator);
        const boost::filesystem::path & names_data_path = paths_iterator->second;

        // get the shared memory segment to use
        bool use_first_segment = SharedMemory::RegionExists( LAYOUT_2 );
        SharedDataType LAYOUT = ( use_first_segment ? LAYOUT_1 : LAYOUT_2 );
        SharedDataType DATA   = ( use_first_segment ? DATA_1 : DATA_2 );

        // Allocate a memory layout in shared memory, deallocate previous
        SharedMemory * layout_memory = SharedMemoryFactory::Get(
            LAYOUT,
            sizeof(SharedDataLayout)
        );
        SharedDataLayout * shared_layout_ptr = static_cast<SharedDataLayout *>(
            layout_memory->Ptr()
        );
        shared_layout_ptr = new(layout_memory->Ptr()) SharedDataLayout();

        std::copy(
            file_index_file_name.begin(),
            (file_index_file_name.length() <= 1024 ? file_index_file_name.end() : file_index_file_name.begin()+1023),
            shared_layout_ptr->ram_index_file_name
        );
        // add zero termination
        unsigned end_of_string_index = std::min(1023ul, file_index_file_name.length());
        shared_layout_ptr->ram_index_file_name[end_of_string_index] = '\0';

        // collect number of elements to store in shared memory object
        SimpleLogger().Write(logDEBUG) << "Collecting files sizes";
        // number of entries in name index
        boost::filesystem::ifstream name_stream(
            names_data_path, std::ios::binary
        );
        unsigned name_index_size = 0;
        name_stream.read((char *)&name_index_size, sizeof(unsigned));
        shared_layout_ptr->name_index_list_size = name_index_size;
        // SimpleLogger().Write() << "name index size: " << shared_layout_ptr->name_index_list_size;
        BOOST_ASSERT_MSG(0 != shared_layout_ptr->name_index_list_size, "name file broken");

        unsigned number_of_chars = 0;
        name_stream.read((char *)&number_of_chars, sizeof(unsigned));
        shared_layout_ptr->name_char_list_size = number_of_chars;
        // SimpleLogger().Write() << "name char size: " << shared_layout_ptr->name_char_list_size;

        //Loading information for original edges
        boost::filesystem::ifstream edges_input_stream(
            edges_data_path,
            std::ios::binary
        );
        unsigned number_of_original_edges = 0;
        edges_input_stream.read((char*)&number_of_original_edges, sizeof(unsigned));

        shared_layout_ptr->via_node_list_size = number_of_original_edges;
        shared_layout_ptr->name_id_list_size = number_of_original_edges;
        shared_layout_ptr->turn_instruction_list_size = number_of_original_edges;

        boost::filesystem::ifstream hsgr_input_stream(
            hsgr_path,
            std::ios::binary
        );

        UUID uuid_loaded, uuid_orig;
        hsgr_input_stream.read((char *)&uuid_loaded, sizeof(UUID));
        if( !uuid_loaded.TestGraphUtil(uuid_orig) ) {
            SimpleLogger().Write(logWARNING) <<
                ".hsgr was prepared with different build. "
                "Reprocess to get rid of this warning.";
        } else {
            SimpleLogger().Write(logDEBUG) << "UUID checked out ok";
        }

        // load checksum
        unsigned checksum = 0;
        hsgr_input_stream.read((char*)&checksum, sizeof(unsigned) );
        shared_layout_ptr->checksum = checksum;
        // load graph node size
        unsigned number_of_graph_nodes = 0;
        hsgr_input_stream.read(
            (char*) &number_of_graph_nodes,
            sizeof(unsigned)
        );

        BOOST_ASSERT_MSG(
            (0 != number_of_graph_nodes),
            "number of nodes is zero"
        );
        shared_layout_ptr->graph_node_list_size = number_of_graph_nodes;

        // load graph edge size
        unsigned number_of_graph_edges = 0;
        hsgr_input_stream.read( (char*) &number_of_graph_edges, sizeof(unsigned) );
        BOOST_ASSERT_MSG(
            0 != number_of_graph_edges,
            "number of graph edges is zero"
        );
        shared_layout_ptr->graph_edge_list_size = number_of_graph_edges;

        // load rsearch tree size
        boost::filesystem::ifstream tree_node_file(
            ram_index_path,
            std::ios::binary
        );

        uint32_t tree_size = 0;
        tree_node_file.read((char*)&tree_size, sizeof(uint32_t));
        shared_layout_ptr->r_search_tree_size = tree_size;

        //load timestamp size
        std::string m_timestamp;
        if( boost::filesystem::exists(timestamp_path) ) {
            boost::filesystem::ifstream timestampInStream( timestamp_path );
            if(!timestampInStream) {
                SimpleLogger().Write(logWARNING) <<
                    timestamp_path << " not found. setting to default";
            } else {
                getline(timestampInStream, m_timestamp);
                timestampInStream.close();
            }
        }
        if(m_timestamp.empty()) {
            m_timestamp = "n/a";
        }
        if(25 < m_timestamp.length()) {
            m_timestamp.resize(25);
        }
        shared_layout_ptr->timestamp_length = m_timestamp.length();

        //load coordinate size
        boost::filesystem::ifstream nodes_input_stream(
            nodes_data_path,
            std::ios::binary
        );
        unsigned coordinate_list_size = 0;
        nodes_input_stream.read((char *)&coordinate_list_size, sizeof(unsigned));
        shared_layout_ptr->coordinate_list_size = coordinate_list_size;

        // allocate shared memory block

        SimpleLogger().Write() << "allocating shared memory of " << shared_layout_ptr->GetSizeOfLayout() << " bytes";
        SharedMemory * shared_memory = SharedMemoryFactory::Get(
            DATA,
            shared_layout_ptr->GetSizeOfLayout()
        );
        char * shared_memory_ptr = static_cast<char *>(shared_memory->Ptr());

        // read actual data into shared memory object //
        // Loading street names
        // SimpleLogger().Write() << "Loading names index and chars from: " << names_data_path.string();
        unsigned * name_index_ptr = (unsigned*)(
            shared_memory_ptr + shared_layout_ptr->GetNameIndexOffset()
        );
        // SimpleLogger().Write(logDEBUG) << "Bytes: " << shared_layout_ptr->name_index_list_size*sizeof(unsigned);

        name_stream.read(
            (char*)name_index_ptr,
            shared_layout_ptr->name_index_list_size*sizeof(unsigned)
        );

        char * name_char_ptr = shared_memory_ptr + shared_layout_ptr->GetNameListOffset();
        name_stream.read(
            name_char_ptr,
            shared_layout_ptr->name_char_list_size*sizeof(char)
        );
        name_stream.close();

        //load original edge information
        NodeID * via_node_ptr = (NodeID *)(
            shared_memory_ptr + shared_layout_ptr->GetViaNodeListOffset()
        );

        unsigned * name_id_ptr = (unsigned *)(
            shared_memory_ptr + shared_layout_ptr->GetNameIDListOffset()
        );

        TurnInstruction * turn_instructions_ptr = (TurnInstruction *)(
            shared_memory_ptr + shared_layout_ptr->GetTurnInstructionListOffset()
        );

        OriginalEdgeData current_edge_data;
        for(unsigned i = 0; i < number_of_original_edges; ++i) {
            // SimpleLogger().Write() << i << "/" << number_of_edges;
            edges_input_stream.read(
                (char*)&(current_edge_data),
                sizeof(OriginalEdgeData)
            );
            via_node_ptr[i] = current_edge_data.viaNode;
            name_id_ptr[i]  = current_edge_data.nameID;
            turn_instructions_ptr[i] = current_edge_data.turnInstruction;
        }
        edges_input_stream.close();

        // Loading list of coordinates
        FixedPointCoordinate * coordinates_ptr = (FixedPointCoordinate *)(
            shared_memory_ptr + shared_layout_ptr->GetCoordinateListOffset()
        );

        NodeInfo current_node;
        for(unsigned i = 0; i < coordinate_list_size; ++i) {
            nodes_input_stream.read((char *)&current_node, sizeof(NodeInfo));
            coordinates_ptr[i] = FixedPointCoordinate(current_node.lat, current_node.lon);
        }
        nodes_input_stream.close();

        //store timestamp
        char * timestamp_ptr = static_cast<char *>(
            shared_memory_ptr + shared_layout_ptr->GetTimeStampOffset()
        );
        std::copy(
            m_timestamp.c_str(),
            m_timestamp.c_str()+m_timestamp.length(),
            timestamp_ptr
        );

        // store search tree portion of rtree
        char * rtree_ptr = static_cast<char *>(
            shared_memory_ptr + shared_layout_ptr->GetRSearchTreeOffset()
        );

        tree_node_file.read(rtree_ptr, sizeof(RTreeNode)*tree_size);
        tree_node_file.close();

        // load the nodes of the search graph
        QueryGraph::_StrNode * graph_node_list_ptr = (QueryGraph::_StrNode*)(
            shared_memory_ptr + shared_layout_ptr->GetGraphNodeListOffset()
        );
        hsgr_input_stream.read(
            (char*) graph_node_list_ptr,
            shared_layout_ptr->graph_node_list_size*sizeof(QueryGraph::_StrNode)
        );

        // load the edges of the search graph
        QueryGraph::_StrEdge * graph_edge_list_ptr = (QueryGraph::_StrEdge *)(
            shared_memory_ptr + shared_layout_ptr->GetGraphEdgeListOffsett()
        );
        hsgr_input_stream.read(
            (char*) graph_edge_list_ptr,
            shared_layout_ptr->graph_edge_list_size*sizeof(QueryGraph::_StrEdge)
        );
        hsgr_input_stream.close();

        //TODO acquire lock
        SharedMemory * data_type_memory = SharedMemoryFactory::Get(
            CURRENT_REGIONS,
            2*sizeof(SharedDataType),
            true,
            false
        );
        SharedDataType * data_type_ptr = static_cast<SharedDataType *>(
            data_type_memory->Ptr()
        );

        data_type_ptr[0] = LAYOUT;
        data_type_ptr[1] = DATA;
        if(use_first_segment) {
            BOOST_ASSERT( DATA   == DATA_1   );
            BOOST_ASSERT( LAYOUT == LAYOUT_1 );
            SharedMemory::Remove(DATA_2);
            SharedMemory::Remove(LAYOUT_2);
        } else {
            BOOST_ASSERT(   DATA == DATA_2   );
            BOOST_ASSERT( LAYOUT == LAYOUT_2 );
            SharedMemory::Remove(DATA_1);
            SharedMemory::Remove(LAYOUT_1);
        }
        SimpleLogger().Write() << "all data loaded";
    } catch(const std::exception & e) {
        SimpleLogger().Write(logWARNING) << "caught exception: " << e.what();
    }

    return 0;
}
<|endoftext|>


<commit_before>// Copyright (c) 2013, Matthew Harvey. All rights reserved.

#ifndef GUARD_debug_log_hpp
#define GUARD_debug_log_hpp

/** @file debug_log.hpp
 *
 * @brief Provides macro for printing to std::clog in debug builds.
 *
 * From someone called "snstrnd" on Stack Overflow.
 */

#include <iostream>

/**Macros to print to std::clog if DEBUG is defined, or else to compile
 * away to nothing if DEBUG is not defined.
 *
 * If DEBUG is defined, JEWEL_DEBUG_LOG acts like std::clog
 * (but prints "LOG: " before the rest of the output),
 * but if DEBUG is not defined, JEWEL_DEBUG_LOG, and the rest
 * of the line, is effectively compiled away to nothing.
 *
 * If DEBUG is defined JEWEL_DEBUG_LOG_LOCATION prints the
 * file and line number to std::clog; otherwise, it is
 * effectively compiled away to nothing.
 *
 * If DEBUG is defined JEWEL_DEBUG_LOG_VALUE(x) prints a string
 * of the form "x: [value of x]". E.g. if x == 3, then
 * JEWEL_DEBUG_LOG_VALUE(x) will print "x: 3".
 *
 * Note a semicolon must still be included by client code
 * at the end of a statement using either of these macros.
 *
 * No particular exception safety guarantee is offered by
 * these macros. They are not intended for use in release builds.
 * But note that all it does is write to std::clog; so unless
 * exceptions have been enabled on std::clog, the only part of this
 * function that could throw is in initialization of the string being
 * written to std::clog. This might throw under conditions of
 * memory shortage; but it is extremely unlikely.
 */

#ifdef DEBUG
	#define JEWEL_DEBUG_LOG std::clog << "LOG: "
	#define JEWEL_DEBUG_LOG_LOCATION std::clog << __FILE__ << ": " << __LINE__ << std::endl;
	#define JEWEL_DEBUG_LOG_VALUE(x) std::clog << #x ": " << x << std::endl;
#else
	#define JEWEL_DEBUG_LOG 0 && std::clog
	#define JEWEL_DEBUG_LOG_LOCATION 0
	#define JEWEL_DEBUG_LOG_VALUE(X) 0
#endif

#endif  // GUARD_debug_log_hpp
<commit_msg>Completely reworked "debug_log.hpp" to provide finer grained control of debug logging behaviour. Note API has completely changed.<commit_after>/** @file debug_log.hpp
 *
 * @brief Debug logging facilities
 *
 * @author Matthew Harvey
 * @date 01 September 2013
 *
 * Copyright (c) 2013, Matthew Harvey. All rights reserved.
 *
 * @todo Documentation and testing.
 */

#include <iostream>

namespace jewel
{

class Log
{
public:
	enum Level
	{
		trace,
		info,
		debug,
		warning,
		error,
		fatal
	};

	static Level threshold()
	{
		return threshold_aux();
	}

	static void set_threshold(Level p_level)
	{
		threshold_aux() = p_level;
		return;
	}

	static void flush()
	{
		std::clog.flush();
		return;
	}

private:

	static Level& threshold_aux()
	{
		static Level ret = debug;
		return ret;
	}
};

#ifdef DEBUG

	#define JEWEL_TEST_FATAL(level) \
			if (level >= Log::fatal) \
			{ \
				std::clog << "OBITUARY:\t"\
				          << "Application terminated due to Log::fatal." \
				          << std::endl; \
				std::terminate(); \
			}

	#define JEWEL_LOG(level, msg) \
		if (level >= Log::threshold()) \
		{ \
			std::clog << "SEVERITY:\t" << level \
					  << "\tMESSAGE:\t" msg \
					  << "\n"; \
			JEWEL_TEST_FATAL(level); \
		}

	#define JEWEL_LOG_LOCATION(level) \
		if (level >= Log::threshold()) \
		{ \
			std::clog << "SEVERITY:\t" << level \
			          << "\tFUNCTION:\t" << __func__ \
			          << "\tFILE:\t" << __FILE__ \
					  << "\tLINE:\t" << __LINE__ \
					  << "\n"; \
			JEWEL_TEST_FATAL(level); \
		}

	#define JEWEL_LOG_VALUE(level, val) \
		if (level >= Log::threshold()) \
		{ \
			std::clog << "SEVERITY:\t" \
			          << "\tEXPRESSION:\t" << #val \
					  << "\tVALUE:\t" << x \
					  << "\n"; \
			JEWEL_TEST_FATAL(level); \
		}

#else

	#define JEWEL_TEST_FATAL(level) 0
	#define JEWEL_LOG(level, msg) 0
	#define JEWEL_LOG_LOCATION(level) 0
	#define JEWEL_LOG_VALUE(level, val) 0

#endif

}  // namespace jewel

<|endoftext|>


<commit_before>// Copyright (c) 2014, Baidu.com, Inc. All Rights Reserved
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
//
// Author: yanshiguang02@baidu.com

#include "logging.h"

#include <assert.h>
#include <boost/bind.hpp>
#include <queue>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <syscall.h>
#include <sys/time.h>
#include <unistd.h>

#include "mutex.h"
#include "thread.h"
#include "timer.h"

namespace baidu {
namespace common {

int g_log_level = INFO;
int64_t g_log_size = 0;
FILE* g_log_file = stdout;
std::string g_log_file_name;
FILE* g_warning_file = NULL;

std::string GetLogName() {
    char buf[30];
    struct timeval tv;
    gettimeofday(&tv, NULL);
    const time_t seconds = tv.tv_sec;
    struct tm t;
    localtime_r(&seconds, &t);
    snprintf(buf, 30,
        "%02d-%02d.%02d:%02d:%02d.%06d",
        t.tm_mon + 1,
        t.tm_mday,
        t.tm_hour,
        t.tm_min,
        t.tm_sec,
        static_cast<int>(tv.tv_usec));
    std::string log_name(g_log_file_name + ".");
    log_name.append(buf);
    return log_name;
}

void SetLogLevel(int level) {
    g_log_level = level;
}

class AsyncLogger {
public:
    AsyncLogger()
      : jobs_(&mu_), done_(&mu_), stopped_(false), size_(0) {
        thread_.Start(boost::bind(&AsyncLogger::AsyncWriter, this));
    }
    ~AsyncLogger() {
        stopped_ = true;
        {
            MutexLock lock(&mu_);
            jobs_.Signal();
        }
        thread_.Join();
        // close fd
    }
    void WriteLog(int log_level, const char* buffer, int32_t len) {
        MutexLock lock(&mu_);
        buffer_queue_.push(make_pair(log_level, new std::string(buffer, len)));
        jobs_.Signal();
    }
    void AsyncWriter() {
        MutexLock lock(&mu_);
        while (1) {
            int loglen = 0;
            int wflen = 0;
            while (!buffer_queue_.empty()) {
                if (g_log_size && size_ > g_log_size) {
                    if (g_log_file != stdout) {
                        fclose(g_log_file);
                        std::string new_log_name = GetLogName();
                        g_log_file = fopen(new_log_name.c_str(), "ab");
                        remove(g_log_file_name.c_str());
                        symlink(new_log_name.c_str(), g_log_file_name.c_str());
                    }
                    size_ = 0;
                }
                int log_level = buffer_queue_.front().first;
                std::string* str = buffer_queue_.front().second;
                buffer_queue_.pop();
                mu_.Unlock();
                if (str && !str->empty()) {
                    fwrite(str->data(), 1, str->size(), g_log_file);
                    loglen += str->size();
                    if (g_warning_file && log_level >= 8) {
                        fwrite(str->data(), 1, str->size(), g_warning_file);
                        wflen += str->size();
                    }
                    if (g_log_size) size_ += str->length();
                }
                delete str;
                mu_.Lock();
            }
            if (loglen) fflush(g_log_file);
            if (wflen) fflush(g_warning_file);
            if (stopped_) {
                break;
            }
            done_.Broadcast();
            jobs_.Wait();
        }
    }
    void Flush() {
        MutexLock lock(&mu_);
        buffer_queue_.push(std::make_pair(0, reinterpret_cast<std::string*>(NULL)));
        jobs_.Signal();
        done_.Wait();
    }
private:
    Mutex mu_;
    CondVar jobs_;
    CondVar done_;
    bool stopped_;
    int64_t size_;
    Thread thread_;
    std::queue<std::pair<int, std::string*> > buffer_queue_;
};

AsyncLogger g_logger;

bool SetWarningFile(const char* path, bool append) {
    const char* mode = append ? "ab" : "wb";
    FILE* fp = fopen(path, mode);
    if (fp == NULL) {
        return false;
    }
    if (g_warning_file) {
        fclose(g_warning_file);
    }
    g_warning_file = fp;
    return true;
}

bool SetLogFile(const char* path, bool append) {
    const char* mode = append ? "ab" : "wb";
    g_log_file_name.assign(path);
    std::string current_log_name = GetLogName();
    FILE* fp = fopen(current_log_name.c_str(), mode);
    if (fp == NULL) {
        g_log_file = stdout;
        return false;
    }
    if (g_log_file != stdout) {
        fclose(g_log_file);
    }
    g_log_file = fp;
    remove(g_log_file_name.c_str());
    symlink(current_log_name.c_str(), g_log_file_name.c_str());
    return true;
}

bool SetLogSize(int size) {
    if (size < 0) {
        return false;
    }
    g_log_size = static_cast<int64_t>(size) << 20;
    return true;
}

void Logv(int log_level, const char* format, va_list ap) {
    const uint64_t thread_id = syscall(__NR_gettid);

    // We try twice: the first time with a fixed-size stack allocated buffer,
    // and the second time with a much larger dynamically allocated buffer.
    char buffer[500];
    for (int iter = 0; iter < 2; iter++) {
        char* base;
        int bufsize;
        if (iter == 0) {
            bufsize = sizeof(buffer);
            base = buffer;
        } else {
            bufsize = 30000;
            base = new char[bufsize];
        }
        char* p = base;
        char* limit = base + bufsize;

        int32_t rlen = timer::now_time_str(p, limit - p);
        p += rlen;
        p += snprintf(p, limit - p, " %lld ", static_cast<long long unsigned int>(thread_id));

        // Print the message
        if (p < limit) {
            va_list backup_ap;
            va_copy(backup_ap, ap);
            p += vsnprintf(p, limit - p, format, backup_ap);
            va_end(backup_ap);
        }

        // Truncate to available space if necessary
        if (p >= limit) {
            if (iter == 0) {
                continue;       // Try again with larger buffer
            } else {
                p = limit - 1;
            }
        }

        // Add newline if necessary
        if (p == base || p[-1] != '\n') {
            *p++ = '\n';
        }

        assert(p <= limit);
        //fwrite(base, 1, p - base, g_log_file);
        //fflush(g_log_file);
        //if (g_warning_file && log_level >= 8) {
        //    fwrite(base, 1, p - base, g_warning_file);
        //    fflush(g_warning_file);
        //}
        g_logger.WriteLog(log_level, base, p - base);
        if (log_level == FATAL) {
            g_logger.Flush();
        }
        if (base != buffer) {
            delete[] base;
        }
        break;
    }
}

void Log(int level, const char* fmt, ...) {
    va_list ap;
    va_start(ap, fmt);

    if (level >= g_log_level) {
        Logv(level, fmt, ap);
    }
    va_end(ap);
    if (level == FATAL) {
        abort();
    }
}

} // namespace common
} // namespace baidu

/* vim: set expandtab ts=4 sw=4 sts=4 tw=100: */
<commit_msg>set log size limit | address comments<commit_after>// Copyright (c) 2014, Baidu.com, Inc. All Rights Reserved
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
//
// Author: yanshiguang02@baidu.com

#include "logging.h"

#include <assert.h>
#include <boost/bind.hpp>
#include <queue>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <syscall.h>
#include <sys/time.h>
#include <unistd.h>

#include "mutex.h"
#include "thread.h"
#include "timer.h"

namespace baidu {
namespace common {

int g_log_level = INFO;
int64_t g_log_size = 0;
FILE* g_log_file = stdout;
std::string g_log_file_name;
FILE* g_warning_file = NULL;

bool GetNewLog(bool append) {
    char buf[30];
    struct timeval tv;
    gettimeofday(&tv, NULL);
    const time_t seconds = tv.tv_sec;
    struct tm t;
    localtime_r(&seconds, &t);
    snprintf(buf, 30,
        "%02d-%02d.%02d:%02d:%02d.%06d",
        t.tm_mon + 1,
        t.tm_mday,
        t.tm_hour,
        t.tm_min,
        t.tm_sec,
        static_cast<int>(tv.tv_usec));
    std::string full_path(g_log_file_name + ".");
    full_path.append(buf);
    size_t idx = full_path.rfind('/');
    if (idx == std::string::npos) {
        idx = 0;
    } else {
        idx += 1;
    }
    const char* mode = append ? "ab" : "wb";
    FILE* fp = fopen(full_path.c_str(), mode);
    if (fp == NULL) {
        return false;
    }
    if (g_log_file != stdout) {
        fclose(g_log_file);
    }
    g_log_file = fp;
    remove(g_log_file_name.c_str());
    symlink(full_path.substr(idx).c_str(), g_log_file_name.c_str());
    return true;
}

void SetLogLevel(int level) {
    g_log_level = level;
}

class AsyncLogger {
public:
    AsyncLogger()
      : jobs_(&mu_), done_(&mu_), stopped_(false), size_(0) {
        thread_.Start(boost::bind(&AsyncLogger::AsyncWriter, this));
    }
    ~AsyncLogger() {
        stopped_ = true;
        {
            MutexLock lock(&mu_);
            jobs_.Signal();
        }
        thread_.Join();
        // close fd
    }
    void WriteLog(int log_level, const char* buffer, int32_t len) {
        MutexLock lock(&mu_);
        buffer_queue_.push(make_pair(log_level, new std::string(buffer, len)));
        jobs_.Signal();
    }
    void AsyncWriter() {
        MutexLock lock(&mu_);
        while (1) {
            int loglen = 0;
            int wflen = 0;
            while (!buffer_queue_.empty()) {
                int log_level = buffer_queue_.front().first;
                std::string* str = buffer_queue_.front().second;
                buffer_queue_.pop();
                if (g_log_file != stdout && g_log_size && size_ + str->length() > g_log_size) {
                    GetNewLog(false);
                    size_ = 0;
                }
                mu_.Unlock();
                if (str && !str->empty()) {
                    fwrite(str->data(), 1, str->size(), g_log_file);
                    loglen += str->size();
                    if (g_warning_file && log_level >= 8) {
                        fwrite(str->data(), 1, str->size(), g_warning_file);
                        wflen += str->size();
                    }
                    if (g_log_size) size_ += str->length();
                }
                delete str;
                mu_.Lock();
            }
            if (loglen) fflush(g_log_file);
            if (wflen) fflush(g_warning_file);
            if (stopped_) {
                break;
            }
            done_.Broadcast();
            jobs_.Wait();
        }
    }
    void Flush() {
        MutexLock lock(&mu_);
        buffer_queue_.push(std::make_pair(0, reinterpret_cast<std::string*>(NULL)));
        jobs_.Signal();
        done_.Wait();
    }
private:
    Mutex mu_;
    CondVar jobs_;
    CondVar done_;
    bool stopped_;
    int64_t size_;
    Thread thread_;
    std::queue<std::pair<int, std::string*> > buffer_queue_;
};

AsyncLogger g_logger;

bool SetWarningFile(const char* path, bool append) {
    const char* mode = append ? "ab" : "wb";
    FILE* fp = fopen(path, mode);
    if (fp == NULL) {
        return false;
    }
    if (g_warning_file) {
        fclose(g_warning_file);
    }
    g_warning_file = fp;
    return true;
}

bool SetLogFile(const char* path, bool append) {
    g_log_file_name.assign(path);
    return GetNewLog(append);
}

bool SetLogSize(int size) {
    if (size < 0) {
        return false;
    }
    g_log_size = static_cast<int64_t>(size) << 20;
    return true;
}

void Logv(int log_level, const char* format, va_list ap) {
    const uint64_t thread_id = syscall(__NR_gettid);

    // We try twice: the first time with a fixed-size stack allocated buffer,
    // and the second time with a much larger dynamically allocated buffer.
    char buffer[500];
    for (int iter = 0; iter < 2; iter++) {
        char* base;
        int bufsize;
        if (iter == 0) {
            bufsize = sizeof(buffer);
            base = buffer;
        } else {
            bufsize = 30000;
            base = new char[bufsize];
        }
        char* p = base;
        char* limit = base + bufsize;

        int32_t rlen = timer::now_time_str(p, limit - p);
        p += rlen;
        p += snprintf(p, limit - p, " %lld ", static_cast<long long unsigned int>(thread_id));

        // Print the message
        if (p < limit) {
            va_list backup_ap;
            va_copy(backup_ap, ap);
            p += vsnprintf(p, limit - p, format, backup_ap);
            va_end(backup_ap);
        }

        // Truncate to available space if necessary
        if (p >= limit) {
            if (iter == 0) {
                continue;       // Try again with larger buffer
            } else {
                p = limit - 1;
            }
        }

        // Add newline if necessary
        if (p == base || p[-1] != '\n') {
            *p++ = '\n';
        }

        assert(p <= limit);
        //fwrite(base, 1, p - base, g_log_file);
        //fflush(g_log_file);
        //if (g_warning_file && log_level >= 8) {
        //    fwrite(base, 1, p - base, g_warning_file);
        //    fflush(g_warning_file);
        //}
        g_logger.WriteLog(log_level, base, p - base);
        if (log_level == FATAL) {
            g_logger.Flush();
        }
        if (base != buffer) {
            delete[] base;
        }
        break;
    }
}

void Log(int level, const char* fmt, ...) {
    va_list ap;
    va_start(ap, fmt);

    if (level >= g_log_level) {
        Logv(level, fmt, ap);
    }
    va_end(ap);
    if (level == FATAL) {
        abort();
    }
}

} // namespace common
} // namespace baidu

/* vim: set expandtab ts=4 sw=4 sts=4 tw=100: */
<|endoftext|>


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

#include "modules/planning/tasks/deciders/rule_based_stop_decider/rule_based_stop_decider.h"

#include <string>
#include <tuple>
#include <vector>

#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/util/common.h"
#include "modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::SLPoint;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Vec2d;

namespace {
// TODO(ALL): temporarily copy the value from lane_follow_stage.cc, will extract
// as a common value for planning later
constexpr double kStraightForwardLineCost = 10.0;
}  // namespace

RuleBasedStopDecider::RuleBasedStopDecider(const TaskConfig &config)
    : Decider(config) {
  CHECK(config.has_rule_based_stop_decider_config());
  rule_based_stop_decider_config_ = config.rule_based_stop_decider_config();
  SetName("RuleBasedStopDecider");
}

apollo::common::Status RuleBasedStopDecider::Process(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  // 1. Rule_based stop for side pass onto reverse lane
  if (FLAGS_enable_nonscenario_side_pass) {
    StopOnSidePass(frame, reference_line_info);
  }

  // 2. Rule_based stop for urgent lane change
  if (FLAGS_enable_lane_change_urgency_checking) {
    CheckLaneChangeUrgency(frame);
  }

  // 3. Rule_based stop at path end position
  AddPathEndStop(frame, reference_line_info);

  return Status::OK();
}

void RuleBasedStopDecider::CheckLaneChangeUrgency(Frame *const frame) {
  for (auto &reference_line_info : *frame->mutable_reference_line_info()) {
    // Check if the target lane is blocked or not
    if (reference_line_info.IsChangeLanePath()) {
      is_clear_to_change_lane_ =
          LaneChangeDecider::IsClearToChangeLane(&reference_line_info);
      is_change_lane_planning_succeed_ =
          reference_line_info.Cost() < kStraightForwardLineCost;
      continue;
    }
    // If it's not in lane-change scenario || (target lane is not blocked &&
    // change lane planning succeed), skip
    if (frame->reference_line_info().size() <= 1 ||
        (is_clear_to_change_lane_ && is_change_lane_planning_succeed_)) {
      continue;
    }
    // When the target lane is blocked in change-lane case, check the urgency
    // Get the end point of current routing
    const auto &route_end_waypoint =
        reference_line_info.Lanes().RouteEndWaypoint();
    // If can't get lane from the route's end waypoint, then skip
    if (!route_end_waypoint.lane) {
      continue;
    }
    auto point = route_end_waypoint.lane->GetSmoothPoint(route_end_waypoint.s);
    auto *reference_line = reference_line_info.mutable_reference_line();
    common::SLPoint sl_point;
    // Project the end point to sl_point on current reference lane
    if (reference_line->XYToSL({point.x(), point.y()}, &sl_point) &&
        reference_line->IsOnLane(sl_point)) {
      // Check the distance from ADC to the end point of current routing
      double distance_to_passage_end =
          sl_point.s() - reference_line_info.AdcSlBoundary().end_s();
      // If ADC is still far from the end of routing, no need to stop, skip
      if (distance_to_passage_end >
          rule_based_stop_decider_config_.approach_distance_for_lane_change()) {
        continue;
      }
      // In urgent case, set a temporary stop fence and wait to change lane
      // TODO(Jiaxuan Xu): replace the stop fence to more intelligent actions
      const std::string stop_wall_id = "lane_change_stop";
      std::vector<std::string> wait_for_obstacles;
      util::BuildStopDecision(
          stop_wall_id, sl_point.s(),
          rule_based_stop_decider_config_.urgent_distance_for_lane_change(),
          StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY, wait_for_obstacles,
          "RuleBasedStopDecider", frame, &reference_line_info);
    }
  }
}

void RuleBasedStopDecider::AddPathEndStop(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  const std::string stop_wall_id = "path_end_stop";
  std::vector<std::string> wait_for_obstacles;
  if (!reference_line_info->path_data().path_label().empty() &&
      reference_line_info->path_data().frenet_frame_path().back().s() -
              reference_line_info->path_data().frenet_frame_path().front().s() <
          FLAGS_short_path_length_threshold) {
    util::BuildStopDecision(
        stop_wall_id,
        reference_line_info->path_data().frenet_frame_path().back().s() - 5.0,
        0.0, StopReasonCode::STOP_REASON_REFERENCE_END, wait_for_obstacles,
        "RuleBasedStopDecider", frame, reference_line_info);
  }
}

void RuleBasedStopDecider::StopOnSidePass(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  const PathData &path_data = reference_line_info->path_data();
  double stop_s_on_pathdata = 0.0;
  const auto &side_pass_status =
      PlanningContext::Instance()->planning_status().side_pass();
  auto *mutable_side_pass_status = PlanningContext::Instance()
                                       ->mutable_planning_status()
                                       ->mutable_side_pass();

  if (path_data.path_label().find("self") != std::string::npos) {
    mutable_side_pass_status->set_check_clear_flag(false);
    mutable_side_pass_status->mutable_change_lane_stop_path_point()->Clear();
    return;
  }

  if (side_pass_status.check_clear_flag() &&
      CheckClearDone(*reference_line_info,
                     side_pass_status.change_lane_stop_path_point())) {
    mutable_side_pass_status->set_check_clear_flag(false);
  }

  if (!side_pass_status.check_clear_flag() &&
      CheckSidePassStop(path_data, *reference_line_info, &stop_s_on_pathdata)) {
    if (!LaneChangeDecider::IsPerceptionBlocked(
            *reference_line_info,
            rule_based_stop_decider_config_.search_beam_length(),
            rule_based_stop_decider_config_.search_beam_radius_intensity(),
            rule_based_stop_decider_config_.search_range(),
            rule_based_stop_decider_config_.is_block_angle_threshold()) &&
        LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
      return;
    }
    if (!CheckADCStop(path_data, *reference_line_info, stop_s_on_pathdata)) {
      if (!BuildSidePassStopFence(
              path_data, stop_s_on_pathdata,
              mutable_side_pass_status->mutable_change_lane_stop_path_point(),
              frame, reference_line_info)) {
        AERROR << "Set side pass stop fail";
      }
    } else {
      if (LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
        mutable_side_pass_status->set_check_clear_flag(true);
      }
    }
  }
}

// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
    const PathData &path_data, const ReferenceLineInfo &reference_line_info,
    double *stop_s_on_pathdata) {
  const std::vector<std::tuple<double, PathData::PathPointType, double>>
      &path_point_decision_guide = path_data.path_point_decision_guide();
  PathData::PathPointType last_path_point_type =
      PathData::PathPointType::UNKNOWN;
  for (const auto &point_guide : path_point_decision_guide) {
    if (last_path_point_type == PathData::PathPointType::IN_LANE &&
        std::get<1>(point_guide) ==
            PathData::PathPointType::OUT_ON_REVERSE_LANE) {
      *stop_s_on_pathdata = std::get<0>(point_guide);
      // Approximate the stop fence s based on the vehicle position
      const auto &vehicle_config =
          common::VehicleConfigHelper::Instance()->GetConfig();
      const double ego_front_to_center =
          vehicle_config.vehicle_param().front_edge_to_center();
      common::PathPoint stop_pathpoint;
      if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
                                          &stop_pathpoint)) {
        AERROR << "Can't get stop point on path data";
        return false;
      }
      const double ego_theta = stop_pathpoint.theta();
      Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
                      ego_front_to_center * std::sin(ego_theta)};
      const Vec2d stop_fence_pose =
          shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
      double stop_l_on_pathdata = 0.0;
      const auto &nearby_path = reference_line_info.reference_line().map_path();
      stop_s_on_pathdata -= nearby_path.GetNearestPoint(
          stop_fence_pose, stop_s_on_pathdata, &stop_l_on_pathdata);
      return true;
    }
    last_path_point_type = std::get<1>(point_guide);
  }
  return false;
}

// @brief Set stop fence for side pass
bool RuleBasedStopDecider::BuildSidePassStopFence(
    const PathData &path_data, const double stop_s_on_pathdata,
    common::PathPoint *stop_point, Frame *const frame,
    ReferenceLineInfo *const reference_line_info) {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);

  if (!path_data.GetPathPointWithRefS(stop_s_on_pathdata, stop_point)) {
    AERROR << "Can't get stop point on path data";
    return false;
  }

  const std::string stop_wall_id = "Side_Pass_Stop";
  std::vector<std::string> wait_for_obstacles;

  const auto &nearby_path = reference_line_info->reference_line().map_path();
  double stop_point_s = 0.0;
  double stop_point_l = 0.0;
  nearby_path.GetNearestPoint({stop_point->x(), stop_point->y()}, &stop_point_s,
                              &stop_point_l);

  util::BuildStopDecision(stop_wall_id, stop_point_s, 0.0,
                          StopReasonCode::STOP_REASON_SIDEPASS_SAFETY,
                          wait_for_obstacles, "RuleBasedStopDecider", frame,
                          reference_line_info);
  return true;
}

// @brief Check if ADV stop at a stop fence
bool RuleBasedStopDecider::CheckADCStop(
    const PathData &path_data, const ReferenceLineInfo &reference_line_info,
    const double stop_s_on_pathdata) {
  common::PathPoint stop_point;
  if (!path_data.GetPathPointWithRefS(stop_s_on_pathdata, &stop_point)) {
    AERROR << "Can't get stop point on path data";
    return false;
  }

  const double adc_speed =
      common::VehicleStateProvider::Instance()->linear_velocity();
  if (adc_speed > rule_based_stop_decider_config_.max_adc_stop_speed()) {
    ADEBUG << "ADC not stopped: speed[" << adc_speed << "]";
    return false;
  }

  // check stop close enough to stop line of the stop_sign
  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const auto &nearby_path = reference_line_info.reference_line().map_path();
  double stop_point_s = 0.0;
  double stop_point_l = 0.0;
  nearby_path.GetNearestPoint({stop_point.x(), stop_point.y()}, &stop_point_s,
                              &stop_point_l);

  const double distance_stop_line_to_adc_front_edge =
      stop_point_s - adc_front_edge_s;

  if (distance_stop_line_to_adc_front_edge >
      rule_based_stop_decider_config_.max_valid_stop_distance()) {
    ADEBUG << "not a valid stop. too far from stop line.";
    return false;
  }

  return true;
}

bool RuleBasedStopDecider::CheckClearDone(
    const ReferenceLineInfo &reference_line_info,
    const common::PathPoint &stop_point) {
  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
  const double adc_start_l = reference_line_info.AdcSlBoundary().start_l();
  const double adc_end_l = reference_line_info.AdcSlBoundary().end_l();
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  reference_line_info.reference_line().GetLaneWidth(
      (adc_front_edge_s + adc_back_edge_s) / 2.0, &lane_left_width,
      &lane_right_width);
  SLPoint stop_sl_point;
  reference_line_info.reference_line().XYToSL({stop_point.x(), stop_point.y()},
                                              &stop_sl_point);
  // use distance to last stop point to determine if needed to check clear
  // again
  if (adc_back_edge_s > stop_sl_point.s()) {
    if (adc_start_l > -lane_right_width || adc_end_l < lane_left_width) {
      return true;
    }
  }
  return false;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>Planning: fix calling of map::Path::GetNearestPoint<commit_after>/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/tasks/deciders/rule_based_stop_decider/rule_based_stop_decider.h"

#include <string>
#include <tuple>
#include <vector>

#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/util/common.h"
#include "modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::SLPoint;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Vec2d;

namespace {
// TODO(ALL): temporarily copy the value from lane_follow_stage.cc, will extract
// as a common value for planning later
constexpr double kStraightForwardLineCost = 10.0;
}  // namespace

RuleBasedStopDecider::RuleBasedStopDecider(const TaskConfig &config)
    : Decider(config) {
  CHECK(config.has_rule_based_stop_decider_config());
  rule_based_stop_decider_config_ = config.rule_based_stop_decider_config();
  SetName("RuleBasedStopDecider");
}

apollo::common::Status RuleBasedStopDecider::Process(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  // 1. Rule_based stop for side pass onto reverse lane
  if (FLAGS_enable_nonscenario_side_pass) {
    StopOnSidePass(frame, reference_line_info);
  }

  // 2. Rule_based stop for urgent lane change
  if (FLAGS_enable_lane_change_urgency_checking) {
    CheckLaneChangeUrgency(frame);
  }

  // 3. Rule_based stop at path end position
  AddPathEndStop(frame, reference_line_info);

  return Status::OK();
}

void RuleBasedStopDecider::CheckLaneChangeUrgency(Frame *const frame) {
  for (auto &reference_line_info : *frame->mutable_reference_line_info()) {
    // Check if the target lane is blocked or not
    if (reference_line_info.IsChangeLanePath()) {
      is_clear_to_change_lane_ =
          LaneChangeDecider::IsClearToChangeLane(&reference_line_info);
      is_change_lane_planning_succeed_ =
          reference_line_info.Cost() < kStraightForwardLineCost;
      continue;
    }
    // If it's not in lane-change scenario || (target lane is not blocked &&
    // change lane planning succeed), skip
    if (frame->reference_line_info().size() <= 1 ||
        (is_clear_to_change_lane_ && is_change_lane_planning_succeed_)) {
      continue;
    }
    // When the target lane is blocked in change-lane case, check the urgency
    // Get the end point of current routing
    const auto &route_end_waypoint =
        reference_line_info.Lanes().RouteEndWaypoint();
    // If can't get lane from the route's end waypoint, then skip
    if (!route_end_waypoint.lane) {
      continue;
    }
    auto point = route_end_waypoint.lane->GetSmoothPoint(route_end_waypoint.s);
    auto *reference_line = reference_line_info.mutable_reference_line();
    common::SLPoint sl_point;
    // Project the end point to sl_point on current reference lane
    if (reference_line->XYToSL({point.x(), point.y()}, &sl_point) &&
        reference_line->IsOnLane(sl_point)) {
      // Check the distance from ADC to the end point of current routing
      double distance_to_passage_end =
          sl_point.s() - reference_line_info.AdcSlBoundary().end_s();
      // If ADC is still far from the end of routing, no need to stop, skip
      if (distance_to_passage_end >
          rule_based_stop_decider_config_.approach_distance_for_lane_change()) {
        continue;
      }
      // In urgent case, set a temporary stop fence and wait to change lane
      // TODO(Jiaxuan Xu): replace the stop fence to more intelligent actions
      const std::string stop_wall_id = "lane_change_stop";
      std::vector<std::string> wait_for_obstacles;
      util::BuildStopDecision(
          stop_wall_id, sl_point.s(),
          rule_based_stop_decider_config_.urgent_distance_for_lane_change(),
          StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY, wait_for_obstacles,
          "RuleBasedStopDecider", frame, &reference_line_info);
    }
  }
}

void RuleBasedStopDecider::AddPathEndStop(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  const std::string stop_wall_id = "path_end_stop";
  std::vector<std::string> wait_for_obstacles;
  if (!reference_line_info->path_data().path_label().empty() &&
      reference_line_info->path_data().frenet_frame_path().back().s() -
              reference_line_info->path_data().frenet_frame_path().front().s() <
          FLAGS_short_path_length_threshold) {
    util::BuildStopDecision(
        stop_wall_id,
        reference_line_info->path_data().frenet_frame_path().back().s() - 5.0,
        0.0, StopReasonCode::STOP_REASON_REFERENCE_END, wait_for_obstacles,
        "RuleBasedStopDecider", frame, reference_line_info);
  }
}

void RuleBasedStopDecider::StopOnSidePass(
    Frame *const frame, ReferenceLineInfo *const reference_line_info) {
  const PathData &path_data = reference_line_info->path_data();
  double stop_s_on_pathdata = 0.0;
  const auto &side_pass_status =
      PlanningContext::Instance()->planning_status().side_pass();
  auto *mutable_side_pass_status = PlanningContext::Instance()
                                       ->mutable_planning_status()
                                       ->mutable_side_pass();

  if (path_data.path_label().find("self") != std::string::npos) {
    mutable_side_pass_status->set_check_clear_flag(false);
    mutable_side_pass_status->mutable_change_lane_stop_path_point()->Clear();
    return;
  }

  if (side_pass_status.check_clear_flag() &&
      CheckClearDone(*reference_line_info,
                     side_pass_status.change_lane_stop_path_point())) {
    mutable_side_pass_status->set_check_clear_flag(false);
  }

  if (!side_pass_status.check_clear_flag() &&
      CheckSidePassStop(path_data, *reference_line_info, &stop_s_on_pathdata)) {
    if (!LaneChangeDecider::IsPerceptionBlocked(
            *reference_line_info,
            rule_based_stop_decider_config_.search_beam_length(),
            rule_based_stop_decider_config_.search_beam_radius_intensity(),
            rule_based_stop_decider_config_.search_range(),
            rule_based_stop_decider_config_.is_block_angle_threshold()) &&
        LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
      return;
    }
    if (!CheckADCStop(path_data, *reference_line_info, stop_s_on_pathdata)) {
      if (!BuildSidePassStopFence(
              path_data, stop_s_on_pathdata,
              mutable_side_pass_status->mutable_change_lane_stop_path_point(),
              frame, reference_line_info)) {
        AERROR << "Set side pass stop fail";
      }
    } else {
      if (LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
        mutable_side_pass_status->set_check_clear_flag(true);
      }
    }
  }
}

// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
    const PathData &path_data, const ReferenceLineInfo &reference_line_info,
    double *stop_s_on_pathdata) {
  const std::vector<std::tuple<double, PathData::PathPointType, double>>
      &path_point_decision_guide = path_data.path_point_decision_guide();
  PathData::PathPointType last_path_point_type =
      PathData::PathPointType::UNKNOWN;
  for (const auto &point_guide : path_point_decision_guide) {
    if (last_path_point_type == PathData::PathPointType::IN_LANE &&
        std::get<1>(point_guide) ==
            PathData::PathPointType::OUT_ON_REVERSE_LANE) {
      *stop_s_on_pathdata = std::get<0>(point_guide);
      // Approximate the stop fence s based on the vehicle position
      const auto &vehicle_config =
          common::VehicleConfigHelper::Instance()->GetConfig();
      const double ego_front_to_center =
          vehicle_config.vehicle_param().front_edge_to_center();
      common::PathPoint stop_pathpoint;
      if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
                                          &stop_pathpoint)) {
        AERROR << "Can't get stop point on path data";
        return false;
      }
      const double ego_theta = stop_pathpoint.theta();
      Vec2d shift_vec{ego_front_to_center * std::cos(ego_theta),
                      ego_front_to_center * std::sin(ego_theta)};
      const Vec2d stop_fence_pose =
          shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
      double stop_l_on_pathdata = 0.0;
      const auto &nearby_path = reference_line_info.reference_line().map_path();
      nearby_path.GetNearestPoint(stop_fence_pose,
                                  stop_s_on_pathdata, &stop_l_on_pathdata);
      return true;
    }
    last_path_point_type = std::get<1>(point_guide);
  }
  return false;
}

// @brief Set stop fence for side pass
bool RuleBasedStopDecider::BuildSidePassStopFence(
    const PathData &path_data, const double stop_s_on_pathdata,
    common::PathPoint *stop_point, Frame *const frame,
    ReferenceLineInfo *const reference_line_info) {
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);

  if (!path_data.GetPathPointWithRefS(stop_s_on_pathdata, stop_point)) {
    AERROR << "Can't get stop point on path data";
    return false;
  }

  const std::string stop_wall_id = "Side_Pass_Stop";
  std::vector<std::string> wait_for_obstacles;

  const auto &nearby_path = reference_line_info->reference_line().map_path();
  double stop_point_s = 0.0;
  double stop_point_l = 0.0;
  nearby_path.GetNearestPoint({stop_point->x(), stop_point->y()}, &stop_point_s,
                              &stop_point_l);

  util::BuildStopDecision(stop_wall_id, stop_point_s, 0.0,
                          StopReasonCode::STOP_REASON_SIDEPASS_SAFETY,
                          wait_for_obstacles, "RuleBasedStopDecider", frame,
                          reference_line_info);
  return true;
}

// @brief Check if ADV stop at a stop fence
bool RuleBasedStopDecider::CheckADCStop(
    const PathData &path_data, const ReferenceLineInfo &reference_line_info,
    const double stop_s_on_pathdata) {
  common::PathPoint stop_point;
  if (!path_data.GetPathPointWithRefS(stop_s_on_pathdata, &stop_point)) {
    AERROR << "Can't get stop point on path data";
    return false;
  }

  const double adc_speed =
      common::VehicleStateProvider::Instance()->linear_velocity();
  if (adc_speed > rule_based_stop_decider_config_.max_adc_stop_speed()) {
    ADEBUG << "ADC not stopped: speed[" << adc_speed << "]";
    return false;
  }

  // check stop close enough to stop line of the stop_sign
  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const auto &nearby_path = reference_line_info.reference_line().map_path();
  double stop_point_s = 0.0;
  double stop_point_l = 0.0;
  nearby_path.GetNearestPoint({stop_point.x(), stop_point.y()}, &stop_point_s,
                              &stop_point_l);

  const double distance_stop_line_to_adc_front_edge =
      stop_point_s - adc_front_edge_s;

  if (distance_stop_line_to_adc_front_edge >
      rule_based_stop_decider_config_.max_valid_stop_distance()) {
    ADEBUG << "not a valid stop. too far from stop line.";
    return false;
  }

  return true;
}

bool RuleBasedStopDecider::CheckClearDone(
    const ReferenceLineInfo &reference_line_info,
    const common::PathPoint &stop_point) {
  const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
  const double adc_start_l = reference_line_info.AdcSlBoundary().start_l();
  const double adc_end_l = reference_line_info.AdcSlBoundary().end_l();
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  reference_line_info.reference_line().GetLaneWidth(
      (adc_front_edge_s + adc_back_edge_s) / 2.0, &lane_left_width,
      &lane_right_width);
  SLPoint stop_sl_point;
  reference_line_info.reference_line().XYToSL({stop_point.x(), stop_point.y()},
                                              &stop_sl_point);
  // use distance to last stop point to determine if needed to check clear
  // again
  if (adc_back_edge_s > stop_sl_point.s()) {
    if (adc_start_l > -lane_right_width || adc_end_l < lane_left_width) {
      return true;
    }
  }
  return false;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>#include <arpc.h>
#include <chord_types.h>
#include <chord_prot.h>
#include <fingers_prot.h>
#include <misc_utils.h>
#include <id_utils.h>
#include "rpclib.h"

int outstanding = 0;

void
printreslist_cb (str desc, chord_nodelistextres *res, clnt_stat status)
{
  if (status) {
    warnx << "no " << desc << ": " << status << "\n";
    return;
  }

  warnx << "=== " << desc << "\n";
  size_t sz = res->resok->nlist.size ();
  vec<chord_node> zs;
  for (size_t i = 0; i < sz; i++) {
    chord_node_ext y = res->resok->nlist[i];
    chord_node z = make_chord_node (y.n);
    warnx << i << ".\t"
	  << z.x << " "
	  << z.r.hostname << " "
	  << z.r.port << " "
	  << z.vnode_num << " "
	  << y.a_lat << " "
	  << y.a_var << " "
	  << y.nrpc
	  << "\n";
  }
  delete res;
  outstanding--;
  if (outstanding == 0)
    exit (0);
}

void
print_successors (const chord_node &dst)
{
  ptr<chordID> ga = New refcounted<chordID> (dst.x);
  chord_nodelistextres *lst = New chord_nodelistextres ();
  doRPC (dst, chord_program_1, CHORDPROC_GETSUCC_EXT,
	 ga, lst,
	 wrap (printreslist_cb, "successors", lst));
  outstanding++;
}

void
print_fingers (const chord_node &dst)
{
  ptr<chordID> ga = New refcounted<chordID> (dst.x);
  chord_nodelistextres *lst = New chord_nodelistextres ();
  doRPC (dst, fingers_program_1, FINGERSPROC_GETFINGERS_EXT,
	 ga, lst,
	 wrap (printreslist_cb, "fingers", lst));
  outstanding++;
}

int
main (int argc, char *argv[])
{
  chord_node dst;

  if (argc != 4)
    fatal << "Usage: nodeq host port vnode\n";

  if (inet_addr (argv[1]) == INADDR_NONE) {
    // yep, this still blocks.
    struct hostent *h = gethostbyname (argv[1]);
    if (!h)
      fatal << "Invalid address or hostname: " << argv[1] << "\n";
    struct in_addr *ptr = (struct in_addr *) h->h_addr;
    dst.r.hostname = inet_ntoa (*ptr);
  } else {
    dst.r.hostname = argv[1];
  }

  dst.r.port = atoi (argv[2]);
  dst.vnode_num = atoi (argv[3]);
  dst.x = make_chordID (dst.r.hostname, dst.r.port, dst.vnode_num);

  print_successors (dst);
  print_fingers (dst);

  amain ();
}

<commit_msg>do reverse lookup on the IPs returned by nodeq ir -r is the last argument<commit_after>#include <arpc.h>
#include <chord_types.h>
#include <chord_prot.h>
#include <fingers_prot.h>
#include <misc_utils.h>
#include <id_utils.h>
#include "rpclib.h"
#include <async.h>
#include <dns.h>

int outstanding = 0;
bool do_reverse_lookup = false;

void
dns_lookup_cb (int i, chord_node z, chord_node_ext y,
	       ptr<hostent> he, int err) {

  if (!err) z.r.hostname = he->h_name;

  warnx << i << ".\t"
	<< z.x << " "
	<< z.r.hostname << " "
	<< z.r.port << " "
	<< z.vnode_num << " "
	<< y.a_lat << " "
	  << y.a_var << " "
	<< y.nrpc
	<< "\n";
  if (do_reverse_lookup) {
    outstanding--;
    if (outstanding == 0)
      exit (0);
  }
}

void
printreslist_cb (str desc, chord_nodelistextres *res, clnt_stat status)
{
  if (status) {
    warnx << "no " << desc << ": " << status << "\n";
    return;
  }

  warnx << "=== " << desc << "\n";
  size_t sz = res->resok->nlist.size ();
  vec<chord_node> zs;
  for (size_t i = 0; i < sz; i++) {
    struct in_addr ar;
    chord_node_ext y = res->resok->nlist[i];
    chord_node z = make_chord_node (y.n);
    inet_aton (z.r.hostname, &ar);
    if (do_reverse_lookup) {
      outstanding++;
      dns_hostbyaddr (ar, wrap (&dns_lookup_cb, i, z, y));
    } else
      dns_lookup_cb (i, z, y, NULL, 1);
  }
  delete res;
  outstanding--;
  if (outstanding == 0)
    exit (0);
}

void
print_successors (const chord_node &dst)
{
  ptr<chordID> ga = New refcounted<chordID> (dst.x);
  chord_nodelistextres *lst = New chord_nodelistextres ();
  doRPC (dst, chord_program_1, CHORDPROC_GETSUCC_EXT,
	 ga, lst,
	 wrap (printreslist_cb, "successors", lst));
  outstanding++;
}

void
print_fingers (const chord_node &dst)
{
  ptr<chordID> ga = New refcounted<chordID> (dst.x);
  chord_nodelistextres *lst = New chord_nodelistextres ();
  doRPC (dst, fingers_program_1, FINGERSPROC_GETFINGERS_EXT,
	 ga, lst,
	 wrap (printreslist_cb, "fingers", lst));
  outstanding++;
}

int
main (int argc, char *argv[])
{
  chord_node dst;
  if (argc != 4) {
    if (strcmp(argv[4], "-r") == 0) do_reverse_lookup = true;
    else
      fatal << "Usage: nodeq host port vnode\n";
  }
  if (inet_addr (argv[1]) == INADDR_NONE) {
    // yep, this still blocks.
    struct hostent *h = gethostbyname (argv[1]);
    if (!h)
      fatal << "Invalid address or hostname: " << argv[1] << "\n";
    struct in_addr *ptr = (struct in_addr *) h->h_addr;
    dst.r.hostname = inet_ntoa (*ptr);
  } else {
    dst.r.hostname = argv[1];
  }

  dst.r.port = atoi (argv[2]);
  dst.vnode_num = atoi (argv[3]);
  dst.x = make_chordID (dst.r.hostname, dst.r.port, dst.vnode_num);

  outstanding++;
  print_successors (dst);
  while (outstanding > 1) acheck ();
  outstanding = 0;
  print_fingers (dst);

  amain ();
}

<|endoftext|>


<commit_before>/*
 * opencog/attention/FocusBoundaryUpdatingAgent.cc
 *
 * Written by Roman Treutlein
 * All Rights Reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program 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 Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <algorithm>
#include <math.h>
#include <time.h>
#include <map>
#include <chrono>

#include <fstream>
#include <stdio.h>

#include <opencog/util/Config.h>
#include <opencog/util/mt19937ar.h>
#include <opencog/attention/atom_types.h>

#include <opencog/atomspace/AtomSpace.h>
#include <opencog/attentionbank/AttentionBank.h>
#include "FocusBoundaryUpdatingAgent.h"

//#define DEBUG

using namespace opencog;
using namespace std::chrono;

FocusBoundaryUpdatingAgent::FocusBoundaryUpdatingAgent(CogServer& cs) :
    Agent(cs), _atq(&cs.getAtomSpace())
{
    // Provide a logger
    setLogger(new opencog::Logger("FocusBoundaryUpdatingAgent.log", Logger::FINE, true));

    _bank = &attentionbank(_as);
    _bank->setAttentionalFocusBoundary(bottomBoundary);
}

void FocusBoundaryUpdatingAgent::run()
{
        afbSize = std::stod(_atq.get_param_value(AttentionParamQuery::af_size));
        decay = std::stod(_atq.get_param_value(AttentionParamQuery::af_decay));
        bottomBoundary = std::stoi(_atq.get_param_value(AttentionParamQuery::af_bottom));
        minAFSize = std::stoi(_atq.get_param_value(AttentionParamQuery::af_min_size));
        maxAFSize = std::stoi(_atq.get_param_value(AttentionParamQuery::af_max_size));

        AttentionValue::sti_t afboundary = _bank->getAttentionalFocusBoundary();
        // Let there always be K top STI valued atoms in the AF.
        HandleSeq hseq = _bank->getTopSTIValuedHandles();
        if( hseq.size() > 0)
            //getTopSTIVlauedHandles function returns Handles in Increasing STI
            //order.
             afboundary = _bank->get_sti(hseq[0]);
       /*
        AttentionValue::sti_t maxsti = _bank->get_max_STI();
        AttentionValue::sti_t minsti = _bank->get_min_STI();

        AttentionValue::sti_t range = maxsti - minsti;

        int newafb = maxsti - (range * afbSize);
        //int oldafb = afboundary;

        afboundary = newafb * decay + (1.0 - decay) * afboundary;

        afboundary = std::max(afboundary, bottomBoundary);

        //printf("NewAfb: %d OldAfb: %d Afb: %d \n",newafb,oldafb,afboundary);
        */

        // Make sure not too many atoms are in the AF.
        HandleSeq afset;
        _bank->get_handle_set_in_attentional_focus(std::back_inserter(afset));
        if(afset.size() > minAFSize ) {
            afboundary = get_cutoff(afset);
        }

        // Set the AF boundary
        _bank->setAttentionalFocusBoundary(afboundary);
}

/**
 * Finds the natural cut off STI value between the minimum and maximum AF set.
 * The natural cutoff here is assumed as the index where the biggest STI
 * difference occured in the decreasingly ordered set of atoms bn Min and Max AF
 * size.
 *
 * @param afset - Atoms in the AF
 * @return the natural cutoff STI value
 */
AttentionValue::sti_t FocusBoundaryUpdatingAgent::get_cutoff(HandleSeq& afset)
{
    auto getSTI = [&](const Handle& h)->AttentionValue::sti_t {
        return _bank->get_sti(h);
    };
    std::sort(afset.begin(), afset.end(),
            [&](const Handle& h1, const Handle& h2)->bool {
                return getSTI(h1) > getSTI(h2);
            });
    HandleSeq afAtoms = HandleSeq(afset.begin() + minAFSize,
                        (afset.size() > maxAFSize ? afset.begin() + maxAFSize : afset.end()));
    int cut_off_index = 0;
    int biggest_diff = -1 ; // diffs are always +ve. so, its okay to initialize it with -ve number.
    constexpr int DIFF_MAGNITUDE = 0.5; // make this a parameter
    for( HandleSeq::size_type i = 0 ; i < afAtoms.size()-1; i++ ){
        int diff = getSTI(afAtoms[i]) - getSTI(afAtoms[i+1]);
        if(diff > biggest_diff*(1 + DIFF_MAGNITUDE )) {
            biggest_diff = diff;
            cut_off_index = i;
        }
    }

    return getSTI(afAtoms[cut_off_index]);
}
<commit_msg>Fix type mismatch<commit_after>/*
 * opencog/attention/FocusBoundaryUpdatingAgent.cc
 *
 * Written by Roman Treutlein
 * All Rights Reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program 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 Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <algorithm>
#include <math.h>
#include <time.h>
#include <map>
#include <chrono>

#include <fstream>
#include <stdio.h>

#include <opencog/util/Config.h>
#include <opencog/util/mt19937ar.h>
#include <opencog/attention/atom_types.h>

#include <opencog/atomspace/AtomSpace.h>
#include <opencog/attentionbank/AttentionBank.h>
#include "FocusBoundaryUpdatingAgent.h"

//#define DEBUG

using namespace opencog;
using namespace std::chrono;

FocusBoundaryUpdatingAgent::FocusBoundaryUpdatingAgent(CogServer& cs) :
    Agent(cs), _atq(&cs.getAtomSpace())
{
    // Provide a logger
    setLogger(new opencog::Logger("FocusBoundaryUpdatingAgent.log", Logger::FINE, true));

    _bank = &attentionbank(_as);
    _bank->setAttentionalFocusBoundary(bottomBoundary);
}

void FocusBoundaryUpdatingAgent::run()
{
        afbSize = std::stod(_atq.get_param_value(AttentionParamQuery::af_size));
        decay = std::stod(_atq.get_param_value(AttentionParamQuery::af_decay));
        bottomBoundary = std::stoi(_atq.get_param_value(AttentionParamQuery::af_bottom));
        minAFSize = std::stoi(_atq.get_param_value(AttentionParamQuery::af_min_size));
        maxAFSize = std::stoi(_atq.get_param_value(AttentionParamQuery::af_max_size));

        AttentionValue::sti_t afboundary = _bank->getAttentionalFocusBoundary();
        // Let there always be K top STI valued atoms in the AF.
        HandleSeq hseq = _bank->getTopSTIValuedHandles();
        if( hseq.size() > 0)
            //getTopSTIVlauedHandles function returns Handles in Increasing STI
            //order.
             afboundary = _bank->get_sti(hseq[0]);
       /*
        AttentionValue::sti_t maxsti = _bank->get_max_STI();
        AttentionValue::sti_t minsti = _bank->get_min_STI();

        AttentionValue::sti_t range = maxsti - minsti;

        int newafb = maxsti - (range * afbSize);
        //int oldafb = afboundary;

        afboundary = newafb * decay + (1.0 - decay) * afboundary;

        afboundary = std::max(afboundary, bottomBoundary);

        //printf("NewAfb: %d OldAfb: %d Afb: %d \n",newafb,oldafb,afboundary);
        */

        // Make sure not too many atoms are in the AF.
        HandleSeq afset;
        _bank->get_handle_set_in_attentional_focus(std::back_inserter(afset));
        if(afset.size() > minAFSize ) {
            afboundary = get_cutoff(afset);
        }

        // Set the AF boundary
        _bank->setAttentionalFocusBoundary(afboundary);
}

/**
 * Finds the natural cut off STI value between the minimum and maximum AF set.
 * The natural cutoff here is assumed as the index where the biggest STI
 * difference occured in the decreasingly ordered set of atoms bn Min and Max AF
 * size.
 *
 * @param afset - Atoms in the AF
 * @return the natural cutoff STI value
 */
AttentionValue::sti_t FocusBoundaryUpdatingAgent::get_cutoff(HandleSeq& afset)
{
    auto getSTI = [&](const Handle& h)->AttentionValue::sti_t {
        return _bank->get_sti(h);
    };
    std::sort(afset.begin(), afset.end(),
            [&](const Handle& h1, const Handle& h2)->bool {
                return getSTI(h1) > getSTI(h2);
            });
    HandleSeq afAtoms = HandleSeq(afset.begin() + minAFSize,
                        (afset.size() > maxAFSize ? afset.begin() + maxAFSize : afset.end()));
    int cut_off_index = 0;
    int biggest_diff = -1 ; // diffs are always +ve. so, its okay to initialize it with -ve number.
    constexpr int DIFF_MAGNITUDE = 0.5; // make this a parameter
    for( int i = 0 ; i < (int)afAtoms.size()-1; i++ ){
        int diff = getSTI(afAtoms[i]) - getSTI(afAtoms[i+1]);
        if(diff > biggest_diff*(1 + DIFF_MAGNITUDE )) {
            biggest_diff = diff;
            cut_off_index = i;
        }
    }

    return getSTI(afAtoms[cut_off_index]);
}
<|endoftext|>