text:
#include "QStruct.h"

//Boost libraries
#include <boost/any.hpp>
#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_symbols.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/bind.hpp>
#include <boost/regex.hpp>

#include <algorithm>
#include <iostream>
#include <fstream>
#include <cctype>
#include <locale>
#include <memory>

//bjam "-sTOOLS=vc-8_0"

using namespace std ;
using namespace boost ;
using namespace boost::spirit::classic;
using namespace boost::lambda;

qTree coreInstance_parser ;

stack < std::shared_ptr<query>> stk ;

// ---------- SET OF TEMP VARIABLES

list < token > lProgram ;
list < field > lSchema ;
std::string sFieldName = "" ;
field::eType fieldType = field::BAD ;
int fieldCount(0);
int flen = 1 ;

bool invalidChar(char c) {
    return !(c >= 32 && c < 128);
}

void stripUnicode(std::string &str) {
    str.erase(remove_if(str.begin(), str.end(), invalidChar), str.end());
}

//
// Source for ltrim/rtrim:
// https://stackoverflow.com/questions/216823/whats-the-best-way-to-trim-stdstring
//

static inline void ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](int ch) {
        return !std::isspace(ch);
    }));
}

static inline void rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(), [](int ch) {
        return !std::isspace(ch);
    }).base(), s.end());
}

//
//  Semantic actions
//

namespace {

#define RECPTOKEN(x)  { lProgram.push_back( token( x ) ) ; }
#define RECPSTRTK(x)  { lProgram.push_back( token( x , string( str,end ) ) ) ; }

    void do_fcall(char const* str, char const* end) {
        string s(str, end);
        s.erase(s.find('('));
        lProgram.push_back(token(CALL, s)) ;
    }

    void do_alias(char const* str, char const* end) {
        sFieldName = string(str, end) ;
        flen = 1 ;
    }

    void do_alias_f(char const* str, char const* end) {
        sFieldName = "Field_" + boost::lexical_cast<std::string> (fieldCount ++);
    }

    void do_flen(int val) {
        flen = val ;
    }

    void do_ftype(char const* str, char const* end) {
        fieldType = field::BAD ;
        string vStr(string(str, end));
        boost::algorithm::to_lower(vStr);

        if ((vStr == "i8") || (vStr == "byte") || (vStr == "char")) {
            fieldType = field::BYTE ;
        } else if ((vStr == "i16") || (vStr == "integer") || (vStr == "int")) {
            fieldType = field::INTEGER ;
        } else if ((vStr == "rational") || (vStr == "rat")) {
            fieldType = field::RATIONAL ;
        } else {
            throw std::invalid_argument(string("Undefined type ") + vStr);
        }
    }

    void do_stream_assign_id(char const* str, char const* end) {
        stk.top()->id = string(str, end);
        stk.top()->filename = "";
    }

    void do_stream_assign_file(char const* str, char const* end) {
        std::string filename = string(str, end);
        ltrim(filename);
        rtrim(filename);
        stk.top()->filename = filename;
    }

    void do_TScan(char const* str, char const* end)  RECPSTRTK(PUSH_TSCAN)
    void do_ID3(char const* str, char const* end)    RECPSTRTK(PUSH_ID3)
    void do_ID2(char const* str, char const* end)    RECPSTRTK(PUSH_ID2)
    void do_ID4(char const* str, char const* end)    RECPSTRTK(PUSH_ID4)
    void do_ID5(char const* str, char const* end)    RECPSTRTK(PUSH_ID5)
    void do_ID1(char const* str, char const* end)    RECPSTRTK(PUSH_ID1)
    void do_IDX(char const* str, char const* end)    RECPSTRTK(PUSH_IDX)
    void do_Stream(char const* str, char const* end) RECPSTRTK(PUSH_STREAM)
    void do_add(char const*, char const*)            RECPTOKEN(ADD)
    void do_subt(char const*, char const*)           RECPTOKEN(SUBTRACT)
    void do_mult(char const*, char const*)           RECPTOKEN(MULTIPLY)
    void do_div(char const*, char const*)            RECPTOKEN(DIVIDE)
    void do_neg(char const*, char const*)            RECPTOKEN(NEGATE)
    void do_band(char const*, char const*)           RECPTOKEN(AND)
    void do_bor(char const*, char const*)            RECPTOKEN(OR)
    void do_bneg(char const*, char const*)           RECPTOKEN(NOT)
    void do_shash(char const* a, char const* b)        RECPTOKEN(STREAM_HASH)
    void do_sdein(char const*, char const*)          RECPTOKEN(STREAM_DEHASH_DIV)
    void do_sdmin(char const*, char const*)          RECPTOKEN(STREAM_DEHASH_MOD)
    void do_sadd(char const*, char const*)           RECPTOKEN(STREAM_ADD)
    void do_sagse(char const*, char const*)          RECPTOKEN(STREAM_AGSE)

    void do_CMP_eq(char const*, char const*)         RECPTOKEN(CMP_EQUAL)
    void do_CMP_lt(char const*, char const*)         RECPTOKEN(CMP_LT)
    void do_CMP_gt(char const*, char const*)         RECPTOKEN(CMP_GT)
    void do_CMP_le(char const*, char const*)         RECPTOKEN(CMP_LE)
    void do_CMP_ge(char const*, char const*)         RECPTOKEN(CMP_GE)
    void do_CMP_neq(char const*, char const*)        RECPTOKEN(CMP_NOT_EQUAL)
    void do_avg(char const*, char const*)            RECPTOKEN(STREAM_AVG)
    void do_min(char const*, char const*)            RECPTOKEN(STREAM_MIN)
    void do_max(char const*, char const*)            RECPTOKEN(STREAM_MAX)
    void do_sumc(char const*, char const*)           RECPTOKEN(STREAM_SUM)

    boost::rational<int> r_val ;

    void pushVal(double val) {
        lProgram.push_back(token(PUSH_VAL, val)) ;
    }

    void pushValI(int val) {
        lProgram.push_back(token(PUSH_VAL, val)) ;
    }

    void pushValR(char const* str, char const* end) {
        lProgram.push_back(token(PUSH_VAL, r_val)) ;
    }

    void do_static_delta(char const*, char const*) {
        stk.top()->rInterval = r_val ;
    }

    void rational_nominator(int rat_nom_param) {
        r_val = rat_nom_param ;
    }

    void rational_denominator(int rat_denom_param) {
        r_val = r_val / rat_denom_param ;
    }

    void rational_irrational(double rat_irrational_param) {
        r_val = Rationalize(rat_irrational_param);
    }

    void do_streamsubstract(char const* str, char const* end) {
        lProgram.push_back(token(STREAM_SUBSTRACT, r_val)) ;
    }

    void do_timemove(int const val) {
        lProgram.push_back(token(STREAM_TIMEMOVE, val)) ;
    }

    void do_select_section(char const*, char const*) {
        stk.top()->lSchema = lSchema ;
        lSchema.clear() ;
    }

    void do_from_section(char const* str, char const* end) {
        stk.top()->lProgram = lProgram ;
        lProgram.clear() ;
    }

    void do_arg_separator(char const* str, char const* end) {
        for (int i = 0 ; i < flen ; i ++) {
            string s(str, end);

            if (flen == 1) {
                lSchema.push_back(field(sFieldName, lProgram, fieldType, s));
            } else {
                string name = sFieldName + "_" + boost::lexical_cast<std::string> (i) ;
                lSchema.push_back(field(name, lProgram, fieldType, s));
            }

            lProgram.clear() ;
        }

        sFieldName = "" ;
        fieldType = field::BAD ;
        flen = 1 ;
    }

    void do_Inner_Stream_Begin(char const*, char const*) {
        stk.push(std::shared_ptr<query> (new query()));
    }

    void do_Inner_Stream_End(char const*, char const*) {
        string streamNameLoc(stk.top()->id);
        stk.pop();
        lProgram.push_back(token(PUSH_STREAM, streamNameLoc));
    };

    void do_print(char const* str, char const* end) {
        string s(str, end);
    }

    void do_reset() {
        lProgram.clear();
        lSchema.clear();
        sFieldName = "" ;
        fieldType = field::BAD ;
    }

    void do_insert_into_schema(char const* str, char const* end) {
        for (auto   &q : coreInstance_parser) {
            if (q.id == (stk.top())->id) {
                throw std::invalid_argument(string("Duplicate stream name:") + q.id);
            }
        }

        if (! stk.top()->id.empty()) {
            coreInstance_parser.push_back(* (stk.top()));
        }

        do_reset();
    }

    void do_aggregate(char const* str, char const* end) {
        string s(str, end);
        stripUnicode(s);
        parse(s.c_str(),
            // Begin grammar
            (
                (
                    as_lower_d["min"][&do_min]
                    | as_lower_d["max"][&do_max]
                    | as_lower_d["avg"][&do_avg]
                    | as_lower_d["sum"][&do_sumc]
                )
                >> ch_p('(')
                >> alnum_p
                >> ch_p(')')
            ),
            //  End grammar
            space_p).full ;
    }

} //End namespace

////////////////////////////////////////////////////////////////////////////
//
//  Our ql_parser grammar
//
////////////////////////////////////////////////////////////////////////////
struct ql_parser : public grammar<ql_parser> {
    template <typename ScannerT>
    struct definition {
        definition(ql_parser const &) {
            keywords =  "avg",
            "min",
            "max",
            "agse",
            "select",
            "from",
            "stream",
            "as"
            ;
            typedef inhibit_case<strlit<>> Token_t;
            //Operators
            chlit<>     EQUAL('=');
            strlit<>    NOT_EQUAL("<>");
            chlit<>     LT('<');
            strlit<>    LE("<=");
            strlit<>    GE(">=");
            chlit<>     GT('>');
            Token_t     IN_BEGIN = as_lower_d["{"];
            Token_t     IN_END = as_lower_d["}"];
            Token_t NUMBER = as_lower_d["number"] ;
            Token_t CHAR = as_lower_d["char"] ;
            Token_t AVG = as_lower_d["avg"] ;
            Token_t MAX = as_lower_d["max"] ;
            Token_t MIN = as_lower_d["min"] ;
            Token_t SUM = as_lower_d["sum"] ;
            Token_t COUNT = as_lower_d["count"] ;
            Token_t CRC = as_lower_d["crc"] ;
            Token_t ISZERO = as_lower_d["iszero"] ;
            Token_t ISNONZERO = as_lower_d["isnonzero"] ;
            Token_t SUMC = as_lower_d["sum"] ;
            Token_t AGSE = as_lower_d["agse"] ;
            Token_t SELECT = as_lower_d["select"] ;
            Token_t DECLARE = as_lower_d["declare"] ;
            Token_t AS = as_lower_d["as"] ;
            Token_t STREAM = as_lower_d["stream"] ;
            Token_t FILE = as_lower_d["file"] ;
            Token_t FROM = as_lower_d["from"] ;
            Token_t NOT = as_lower_d["not"];
            Token_t AND = as_lower_d["and"];
            Token_t OR = as_lower_d["or"];
            Token_t SQRT = as_lower_d["sqrt"] ;
            Token_t CEIL = as_lower_d["ceil"] ;
            Token_t ABS = as_lower_d["abs"] ;
            Token_t FLOOR = as_lower_d["floor"] ;
            Token_t SIGN = as_lower_d["sign"] ;
            Token_t CHR = as_lower_d["chr"] ;
            Token_t LENGTH = as_lower_d["length"] ;
            Token_t TO_CHAR = as_lower_d["to_char"] ;
            Token_t TO_NUMBER = as_lower_d["to_number"] ;
            Token_t TO_TIMESTAMP = as_lower_d["to_timestamp"] ;
            Token_t FLOAT = as_lower_d["float"] ;
            Token_t INT = as_lower_d["int"] ;
            command
                =
                    remarque_command
                    | select_command
                    | declare_command
                    | empty_line
                    ;
            empty_line
                =
                    (
                        * (ch_p(' ') | ch_p('\t'))
                    )
                    ;
            remarque_command
                =
                    (
                        (ch_p('#'))
                        >> * anychar_p
                    )
                    ;
            declare_command
                =
                    (
                        DECLARE
                        >>
                        (
                            (
                                (
                                    id[&do_alias] >> !
                                    (
                                        (id [&do_ftype])
                                        >> !
                                        (ch_p('[') >> uint_p[&do_flen] >> ch_p(']'))
                                    )
                                )                               [&do_arg_separator]
                            ) % ch_p(',')
                        )                                       [&do_select_section]
                        >>
                        (
                            STREAM
                            >> id                               [&do_stream_assign_id]
                            >> ch_p(',') >> (rational           [&do_static_delta]
                            )
                        )
                        >>
                        (
                            FILE
                            >> (* anychar_p)                    [&do_stream_assign_file]
                        )
                    )
                    [&do_insert_into_schema]
                    ;
            select_command
                =
                    (
                        SELECT
                        >>
                        (
                            (
                                table_scan                     [&do_alias_f]
                                | (expression                  [&do_alias_f]
                                    >> !(AS >> id[&do_alias])
                                )
                            )                                  [&do_arg_separator]
                            % ch_p(',')
                        )                                      [&do_select_section]
                        >> (STREAM >> id[&do_stream_assign_id])
                        >> (FROM >> stream_expression)       [&do_from_section]
                    )
                    [&do_insert_into_schema]
                    ;
            rational
                =
                    (uint_p [&rational_nominator] >> ch_p('/') >> uint_p[&rational_denominator])
                    | ch_p('(') >> rational >> ch_p(')')
                    | real_p[&rational_irrational]
                    ;
            table_scan
                =
                    (!(stream_id >> ch_p('.')) >> ch_p('*'))       [&do_TScan]
                    ;
            condition
                =
                    !(NOT [ &do_bneg])
                    >> logical_term
                    >> * ((OR >> logical_term)                       [ &do_bor ]
                    )
                    ;
            logical_term
                =
                    logical_factor >>  * ((AND >> logical_factor)     [ &do_band ])
                    ;
            logical_factor
                =
                    (ch_p('(') >> condition >> ch_p(')'))
                    | expression >>
                    (
                        (EQUAL >> expression)         [&do_CMP_eq]
                        | (NOT_EQUAL >> expression)   [&do_CMP_neq]
                        | (GT >> expression)          [&do_CMP_gt]
                        | (LT >> expression)          [&do_CMP_lt]
                        | (GE >> expression)          [&do_CMP_ge]
                        | (LE >> expression)          [&do_CMP_le]
                    )
                    ;
            stream_id
                =
                    id
                    ;
            field_id
                =
                    (
                        (id >> ch_p('.') >> id)                                   [&do_ID1]      //a stream.field
                        | (id >> ch_p('[') >> uint_p >> ch_p(']') >> ch_p('[') >> uint_p >> ch_p(']')) [&do_ID5]               //b stream[idx][timemove]
                        | (id >> ch_p('[') >> uint_p >> ch_p(',') >> uint_p >> ch_p(']')) [&do_ID4]            //b stream[idx,timemove]
                        | (id >> ch_p('[') >> uint_p >> ch_p(']'))                   [&do_ID2]         //b stream[idx]
                        | (id >> ch_p('[') >> ch_p('_') >> ch_p(']'))                [&do_IDX]
                        | id                                                     [&do_ID3]  //a field
                    )
                    ;
            id
                =
                    as_lower_d
                    [
                        lexeme_d
                        [
                            (alpha_p >> * (alnum_p | '_'))
                            - (keywords >> anychar_p - (alnum_p | '_'))
                        ]
                    ]
                    ;
            stream_expression
                =
                    stream_term >>
                    * (
                        (ch_p('>') >> int_p[&do_timemove])
                        | (ch_p('+') >> stream_term)          [&do_sadd]
                        | (ch_p('-') >> rational)             [&do_streamsubstract]
                    )
                    ;
            stream_term
                =
                    stream_factor >>
                    * (
                        (ch_p('#') >> stream_factor)          [&do_shash]
                        | (ch_p('&') >> rational[&pushValR])  [&do_sdein]
                        | (ch_p('%') >> rational[&pushValR])  [&do_sdmin]
                        | (
                            ch_p('@')
                            >> ch_p('(') >> uint_p              [&pushValI]
                            >> ch_p(',') >> int_p               [&pushValI]
                            >> ch_p(')')
                        )                                       [&do_sagse]
                        | (ch_p('.') >> agregator)
                    )
                    ;
            agregator
                = (MIN           [&do_min]
                        | MAX       [&do_max]
                        | AVG       [&do_avg]
                        | SUMC      [&do_sumc]
                    )
                    ;
            stream_factor
                =
                    stream_id                                   [&do_Stream]
                    |   ch_p('(') >> stream_expression >> ch_p(')')
                    | (
                        IN_BEGIN                             [&do_Inner_Stream_Begin]
                        >> select_command
                        >> IN_END
                    )                                       [&do_Inner_Stream_End]
                    ;
            expression
                =
                    term
                    >> * (
                        ('+' >> term)               [&do_add]
                        | ('-' >> term)               [&do_subt]
                    )
                    ;
            term
                =   factor
                    >> * (
                        ('*' >> factor)             [&do_mult]
                        | ('/' >> factor)             [&do_div]
                    )
                    ;
            factor
                =   real_p                      [&pushVal]
                    | ('-' >> factor)             [&do_neg]
                    | ('+' >> factor)
                    |   funct
                    |   field_id
                    |   agregator                   [&do_aggregate]
                    |   '(' >> expression >> ')'
                    ;
            funct
                =
                    (
                        (SQRT
                            | CEIL
                            | ABS
                            | FLOOR
                            | SIGN
                            | CHR
                            | LENGTH
                            | TO_NUMBER
                            | TO_TIMESTAMP
                            | FLOAT
                            | INT
                            | COUNT
                            | CRC
                            | SUM
                            | ISZERO
                            | ISNONZERO
                        )
                        >> ch_p('(') >> * ( expression % ch_p(',') ) >> ch_p(')')
                    )
                    [&do_fcall]
                    ;
            schema
                =
                    ch_p('(')
                    >> field_id % ch_p(',')
                    >> ch_p(')')
                    ;
        }

        symbols<> keywords;
        rule<ScannerT>
        expression, term, factor, agregator,
                    select_command,
                    declare_command,
                    remarque_command,
                    empty_line,
                    id, stream_id, field_id,
                    stream_expression, stream_term, stream_factor, inner_stream,
                    natural, schema, type_id,
                    funct,
                    rational,
                    condition, logical_term, logical_factor,
                    table_scan,
                    command
                    ;

        rule<ScannerT> const &
        start() const {
            return command;
        }
    };
};

string getParseResult() {
    const qTree coreInstance2(coreInstance_parser) ;
    std::stringstream retval;
    boost::archive::text_oarchive oa(retval);
    oa << coreInstance2 ;
    return retval.str();
}

string storeParseResult(string sOutputFile) {
    // Store of compiled queries on disk
    const qTree coreInstance2(coreInstance_parser) ;
    std::ofstream ofs(sOutputFile.c_str());

    if (! ofs.good()) {
        cerr << "No serialization file:" << sOutputFile << endl ;
        throw std::invalid_argument("Runtime Error");
    }

    boost::archive::text_oarchive oa(ofs);

    if (! ofs.good()) {
        cerr << "File chain serialization fail" << endl ;
        throw std::invalid_argument("Runtime Error");
    }

    oa << coreInstance2 ;
    return string("OK");
}

string parser(vector<string> vsInputFile) {
    //
    // Main parser body
    //
    ql_parser g;
    stk.push(std::shared_ptr<query> (new query()));
    string str;

    for (string line : vsInputFile) {
        do_reset();
        stripUnicode(line);

        if (! parse(line.c_str(), g, space_p).full) {
            cerr << "error in:[" << line << "]" << endl ;
            throw std::invalid_argument("Syntax Error");
        }
    }

    assert(! stk.empty());
    stk.pop();
    return string("OK");
}

max_stars_repo_name: fossabot/retractordb

max_issues_repo_name: fossabot/retractordb

max_forks_repo_name: fossabot/retractordb


text:
/*
 *
 *  Created on: Feb 8, 2015
 *      Author: Lanners Luc
 *
 */

#include "tidyup_state_creators/goalCreatorLoadTablesAsMeshIntoPlanningScene.h"
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h>
#include <shape_tools/solid_primitive_dims.h>
#include <geometric_shapes/shapes.h>
#include <geometric_shapes/mesh_operations.h>
#include <geometric_shapes/shape_messages.h>
#include <geometric_shapes/shape_operations.h>

#include <boost/foreach.hpp>
#define for_each BOOST_FOREACH
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <ros/package.h>

PLUGINLIB_EXPORT_CLASS(tidyup_state_creators::GoalCreatorLoadTablesAsMeshIntoPlanningScene, continual_planning_executive::GoalCreator)

namespace tidyup_state_creators
{
	GoalCreatorLoadTablesAsMeshIntoPlanningScene::GoalCreatorLoadTablesAsMeshIntoPlanningScene()
	{
		ros::NodeHandle nh;
		pubPlanningScene_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);

		ROS_INFO("GoalCreatorLoadTablesAsMeshIntoPlanningScene::%s: Waiting for planning_scene publisher "
				"to have subscribers.", __func__);
		while(pubPlanningScene_.getNumSubscribers() < 1) {
			ros::Duration(0.5).sleep();
		}
	}

	GoalCreatorLoadTablesAsMeshIntoPlanningScene::~GoalCreatorLoadTablesAsMeshIntoPlanningScene()
	{
	}

	void GoalCreatorLoadTablesAsMeshIntoPlanningScene::initialize(const std::deque<std::string> & arguments)
	{
		ROS_ASSERT(arguments.size() == 1);
		type_table_ = arguments[0];
	}

	bool GoalCreatorLoadTablesAsMeshIntoPlanningScene::fillStateAndGoal(SymbolicState & currentState, SymbolicState & goal)
	{
		std::vector<TableLocation> tables;
		if (!symbolic_planning_utils::LoadTables::getTables(tables))
		{
			ROS_ERROR("GoalCreatorLoadTablesAsMeshIntoPlanningScene::%s: Could not load tables", __func__);
			return false;
		}
		loadTablesIntoPlanningScene(tables);

		// Add tables to current state.
		for_each(const TableLocation& tl, tables)
		{
			const string& tablename = tl.name;
			currentState.addObject(tablename, type_table_);
			currentState.setNumericalFluent("timestamp", tablename, tl.pose.header.stamp.toSec());
			currentState.addObject(tl.pose.header.frame_id, "frameid");
			currentState.setObjectFluent("frame-id", tablename, tl.pose.header.frame_id);
			currentState.setNumericalFluent("x", tablename, tl.pose.pose.position.x);
			currentState.setNumericalFluent("y", tablename, tl.pose.pose.position.y);
			currentState.setNumericalFluent("z", tablename, tl.pose.pose.position.z);
			currentState.setNumericalFluent("qx", tablename, tl.pose.pose.orientation.x);
			currentState.setNumericalFluent("qy", tablename, tl.pose.pose.orientation.y);
			currentState.setNumericalFluent("qz", tablename, tl.pose.pose.orientation.z);
			currentState.setNumericalFluent("qw", tablename, tl.pose.pose.orientation.w);
		}

		return true;
	}

	void GoalCreatorLoadTablesAsMeshIntoPlanningScene::loadTablesIntoPlanningScene(const std::vector<TableLocation>& tables)
	{
		ros::NodeHandle nhPriv("~");
		moveit_msgs::PlanningScene planning_scene;
		planning_scene.is_diff = true;

		for_each(const TableLocation& tl, tables)
		{
			std::string param = "table_meshes/" + tl.name;
			std::string param_package = param + "/package";
			std::string package;
			if (!nhPriv.getParam(param_package, package))
			{
				ROS_ERROR("GoalCreatorLoadTablesAsMeshIntoPlanningScene::%s: Could not load mesh for table %s", __func__, tl.name.c_str());
				continue;
			}
			package = ros::package::getPath(package);

			std::string param_path = param + "/path";
			std::string path;
			if (!nhPriv.getParam(param_path, path))
			{
				ROS_ERROR("GoalCreatorLoadTablesAsMeshIntoPlanningScene::%s: Could not load mesh for table %s", __func__, tl.name.c_str());
				continue;
			}

			std::string file_name = package + "/" + path;
			ROS_INFO("GoalCreatorLoadTablesAsMeshIntoPlanningScene::%s: Path to table mesh: %s", __func__, file_name.c_str());

			rosbag::Bag bag;
			bag.open(file_name, rosbag::bagmode::Read);

			std::string topic = "export_table";

			rosbag::View view(bag, rosbag::TopicQuery(topic));
			moveit_msgs::CollisionObject obj;
			for_each(rosbag::MessageInstance const m, view)
			{
				moveit_msgs::CollisionObject::ConstPtr co = m.instantiate<moveit_msgs::CollisionObject>();
				if (co != NULL)
				{
					obj = *co;
					obj.operation = moveit_msgs::CollisionObject::ADD;
				}
			}
			planning_scene.world.collision_objects.push_back(obj);

			// Set colors
			moveit_msgs::ObjectColor oc;
			oc.id = obj.id;
			oc.color.r = 0.67;
			oc.color.g = 0.33;
			oc.color.b = 0.0;
			oc.color.a = 1.0;
			planning_scene.object_colors.push_back(oc);
			bag.close();
		}
		pubPlanningScene_.publish(planning_scene);
	}

};

ext: cpp


text:
/* Copyright 2008, 2009 (C) Nicira, Inc.
 *
 * This file is part of NOX.
 *
 * NOX is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * NOX 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 NOX.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "routing.hh"
#include "openflow-default.hh"

#include <boost/bind.hpp>
#include <inttypes.h>

#include "assert.hh"
#include "openflow/nicira-ext.h"
#include "vlog.hh"
#include "openflow-pack.hh"

namespace vigil {
namespace applications {

static Vlog_module lg("routing");

std::size_t
Routing_module::ridhash::operator()(const RouteId& rid) const
{
    HASH_NAMESPACE::hash<datapathid> dphash;
    return (dphash(rid.src) ^ dphash(rid.dst));
}

bool
Routing_module::rideq::operator()(const RouteId& a, const RouteId& b) const
{
    return (a.src == b.src && a.dst == b.dst);
}

std::size_t
Routing_module::routehash::operator()(const RoutePtr& rte) const
{
    return (ridhash()(rte->id)
            ^ HASH_NAMESPACE::hash<uint32_t>()(rte->path.size()));
}

bool
Routing_module::routeq::operator()(const RoutePtr& a, const RoutePtr& b) const
{
    if (a->id.src != b->id.src || a->id.dst != b->id.dst
        || a->path.size() != b->path.size())
    {
        return false;
    }
    std::list<Link>::const_iterator aiter, biter;
    for (aiter = a->path.begin(), biter = b->path.begin();
         aiter != a->path.end(); ++aiter, ++biter)
    {
        if (aiter->dst != biter->dst || aiter->outport != biter->outport
            || aiter->inport != biter->inport)
        {
            return false;
        }
    }
    return true;
}

bool
Routing_module::ruleptrcmp::operator()(const RoutePtr& a, const RoutePtr& b) const
{
    return (a->path.size() > b->path.size());
}

static
inline
uint32_t
get_max_action_len() {
    if (sizeof(ofp_action_output) >= sizeof(nx_action_snat)) {
        return sizeof(ofp_action_output)+sizeof(ofp_action_dl_addr);
    }
    return sizeof(nx_action_snat)+sizeof(ofp_action_dl_addr);
}

// Constructor - initializes openflow packet memory used to setup route

Routing_module::Routing_module(const container::Context* c,
                               const json_object* d)
    : container::Component(c), topology(0), nat(0), len_flow_actions(0),
      num_actions(0), ofm(0)
{
    max_output_action_len = get_max_action_len();
}

void
Routing_module::getInstance(const container::Context* ctxt,
                            Routing_module*& r)
{
    r = dynamic_cast<Routing_module*>
        (ctxt->get_by_interface(container::Interface_description
                                (typeid(Routing_module).name())));
}

void
Routing_module::configure(const container::Configuration*)
{
    resolve(topology);
    resolve(nat);
    register_handler<Link_event>
        (boost::bind(&Routing_module::handle_link_change, this, _1));
}

void
Routing_module::install()
{}

bool
Routing_module::get_route(const RouteId& id, RoutePtr& route) const
{
    RouteMap::const_iterator rte = shortest.find(id);
    if (rte == shortest.end()) {
        if (id.src == id.dst) {
            route.reset(new Route());
            route->id = id;
            return true;
        }
        return false;
    }

    route = rte->second;
    return true;
}

bool
Routing_module::check_route(const Route& route, uint16_t inport,
                            uint16_t outport) const
{
    if (route.path.empty()) {
        if (inport == outport) {
            return false;
        }
    } else if (inport == route.path.front().outport) {
        return false;
    } else if (outport == route.path.back().inport) {
        return false;
    }
    return true;
}

bool
Routing_module::is_on_path_location(const RouteId& id, uint16_t src_port,
                                    uint16_t dst_port)
{
    Routing_module::RoutePtr rte;
    return (id.src != id.dst
            && get_route(id, rte)
            && rte->path.front().outport != src_port
            && rte->path.back().inport == dst_port);
}

// Updates shortests paths on link change based on algorithm in "A New Approach
// to Dynamic All Pairs Shortest Paths" - C. Demetrescu

Disposition
Routing_module::handle_link_change(const Event& e)
{
    const Link_event& le = assert_cast<const Link_event&>(e);

    RouteQueue new_candidates;
    RoutePtr route(new Route());
    Link tmp = { le.dpdst, le.sport, le.dport };
    route->id.src = le.dpsrc;
    route->id.dst = le.dpdst;
    route->path.push_back(tmp);
    if (le.action == Link_event::REMOVE) {
        cleanup(route, true);
        fixup(new_candidates, true);
    } else if (le.action == Link_event::ADD) {
        RoutePtr left_subpath(new Route());
        RoutePtr right_subpath(new Route());
        left_subpath->id.src = left_subpath->id.dst = le.dpsrc;
        right_subpath->id.src = right_subpath->id.dst = le.dpdst;
        add(local_routes, route);
        add(left_local, route, right_subpath);
        add(right_local, route, left_subpath);
        new_candidates.push(route);
        fixup(new_candidates, false);
    } else {
        VLOG_ERR(lg, "Unknown link event action %u", le.action);
    }

    return CONTINUE;
}

void
Routing_module::cleanup(RoutePtr route, bool delete_route)
{
    bool is_short = remove(shortest, route);
    if (delete_route) {
        remove(local_routes, route);
    }

    RoutePtr subpath(new Route());
    *subpath = *route;
    set_subpath(subpath, true);
    if (delete_route) {
        remove(right_local, route, subpath);
    }
    if (is_short) {
        remove(right_shortest, route, subpath);
    }

    *subpath = *route;
    set_subpath(subpath, false);
    if (delete_route) {
        remove(left_local, route, subpath);
    }
    if (is_short) {
        remove(left_shortest, route, subpath);
    }

    RouteSet to_clean;
    subpath = route;
    while (true) {
        RouteList left, right;
        ExtensionMap::iterator rtes = left_local.find(subpath);
        if (rtes != left_local.end()) {
            left_shortest.erase(subpath);
            left.swap(rtes->second);
            left_local.erase(rtes);
        }
        rtes = right_local.find(subpath);
        if (rtes != right_local.end()) {
            right_shortest.erase(subpath);
            right.swap(rtes->second);
            right_local.erase(rtes);
        }

        clean_subpath(subpath, left, to_clean, true);
        clean_subpath(subpath, right, to_clean, false);

        if (to_clean.empty())
            break;
        subpath = *(to_clean.begin());
        to_clean.erase(to_clean.begin());
    }
}

void
Routing_module::clean_subpath(RoutePtr& subpath, const RouteList& extensions,
                              RouteSet& to_clean, bool is_left_extension)
{
    datapathid tmpdp;
    Link tmplink;

    if (is_left_extension) {
        tmplink = subpath->path.back();
        subpath->path.pop_back();
        if (subpath->path.empty()) {
            subpath->id.dst = subpath->id.src;
        } else {
            subpath->id.dst = subpath->path.back().dst;
        }
    } else {
        tmplink = subpath->path.front();
        subpath->path.pop_front();
        tmpdp = subpath->id.src;
        subpath->id.src = tmplink.dst;
    }

    for (RouteList::const_iterator rte = extensions.begin();
         rte != extensions.end(); ++rte)
    {
        clean_route(*rte, subpath, is_left_extension);
        to_clean.insert(*rte);
    }

    if (is_left_extension) {
        subpath->path.push_back(tmplink);
        subpath->id.dst = tmplink.dst;
    } else {
        subpath->path.push_front(tmplink);
        subpath->id.src = tmpdp;
    }
}

void
Routing_module::clean_route(const RoutePtr& route,
                            RoutePtr& subpath,
                            bool cleaned_left)
{
    datapathid tmpdp;

    if (remove(local_routes, route)) {
        bool is_short = remove(shortest, route);
        if (cleaned_left) {
            tmpdp = subpath->id.src;
            subpath->id.src = route->id.src;
            subpath->path.push_front(route->path.front());
            remove(right_local, route, subpath);
            if (is_short)
                remove(right_shortest, route, subpath);
            subpath->id.src = tmpdp;
            subpath->path.pop_front();
        } else {
            const Link& newlink = route->path.back();
            tmpdp = subpath->id.dst;
            subpath->id.dst = newlink.dst;
            subpath->path.push_back(newlink);
            remove(left_local, route, subpath);
            if (is_short)
                remove(left_shortest, route, subpath);
            subpath->id.dst = tmpdp;
            subpath->path.pop_back();
        }
    }
}

void
Routing_module::fixup(RouteQueue& new_candidates, bool add_least)
{
    if (add_least) {
        for (RoutesMap::iterator rtes = local_routes.begin();
             rtes != local_routes.end(); ++rtes)
        {
            new_candidates.push(*(rtes->second.begin()));
        }
    }

    while (!new_candidates.empty()) {
        RoutePtr route = new_candidates.top();
        new_candidates.pop();
        RouteMap::iterator old = shortest.find(route->id);
        if (old != shortest.end()) {
            if (old->second->path.size() <= route->path.size()) {
                continue;
            }
            cleanup(old->second, false);
        } else if (route->id.src == route->id.dst) {
            continue;
        }

        RoutePtr left_subpath(new Route());
        RoutePtr right_subpath(new Route());
        *left_subpath = *right_subpath = *route;
        set_subpath(left_subpath, true);
        set_subpath(right_subpath, false);
        get_cached_path(left_subpath);
        get_cached_path(right_subpath);
        add(shortest, route);
        add(left_shortest, route, right_subpath);
        add(right_shortest, route, left_subpath);
        add_local_routes(route, left_subpath, right_subpath, new_candidates);
    }
}

void
Routing_module::add_local_routes(const RoutePtr& route,
                                 const RoutePtr& left_subpath,
                                 const RoutePtr& right_subpath,
                                 RouteQueue& new_candidates)
{
    ExtensionMap::iterator rtes = left_shortest.find(left_subpath);
    if (rtes != left_shortest.end()) {
        for (RouteList::const_iterator rte = rtes->second.begin();
             rte != rtes->second.end(); ++rte)
        {
            RoutePtr new_local(new Route());
            *new_local = *route;
            new_local->id.src = (*rte)->id.src;
            new_local->path.push_front((*rte)->path.front());
            add(local_routes, new_local);
            add(left_local, new_local, route);
            add(right_local, new_local, *rte);
            new_candidates.push(new_local);
        }
    }
    rtes = right_shortest.find(right_subpath);
    if (rtes != right_shortest.end()) {
        for (RouteList::const_iterator rte = rtes->second.begin();
             rte != rtes->second.end(); ++rte)
        {
            RoutePtr new_local(new Route());
            *new_local = *route;
            new_local->id.dst = (*rte)->id.dst;
            new_local->path.push_back((*rte)->path.back());
            add(local_routes, new_local);
            add(left_local, new_local, *rte);
            add(right_local, new_local, route);
            new_candidates.push(new_local);
        }
    }
}

void
Routing_module::set_subpath(RoutePtr& subpath, bool left)
{
    if (left) {
        subpath->path.pop_back();
        if (subpath->path.empty()) {
            subpath->id.dst = subpath->id.src;
            return;
        }
        subpath->id.dst = subpath->path.back().dst;
    } else {
        subpath->id.src = subpath->path.front().dst;
        subpath->path.pop_front();
    }
}

void
Routing_module::get_cached_path(RoutePtr& route)
{
    RoutesMap::iterator rtes = local_routes.find(route->id);
    if (rtes == local_routes.end()) {
        // for (dp, dp) paths
        ExtensionMap::iterator check = left_local.find(route);
        if (check != left_local.end()) {
            route = check->first;
        }
        return;
    }

    for (RouteList::const_iterator rte = rtes->second.begin();
         rte != rtes->second.end(); ++rte)
    {
        if (routeq()(route, *rte)) {
            route = *rte;
            return;
        }
    }
}

bool
Routing_module::remove(RouteMap& routes, const RoutePtr& route)
{
    RouteMap::iterator rtes = routes.find(route->id);
    if (rtes != routes.end()) {
        if (routeq()(route, rtes->second)) {
            routes.erase(rtes);
            return true;
        }
    }
    return false;
}

bool
Routing_module::remove(RoutesMap& routes, const RoutePtr& route)
{
    RoutesMap::iterator rtes = routes.find(route->id);
    if (rtes != routes.end()) {
        for (RouteList::iterator rte = rtes->second.begin();
             rte != rtes->second.end(); ++rte)
        {
            if (routeq()(route, *rte)) {
                rtes->second.erase(rte);
                if (rtes->second.empty())
                    routes.erase(rtes);
                return true;
            }
        }
    }
    return false;
}

bool
Routing_module::remove(ExtensionMap& routes, const RoutePtr& route,
                       const RoutePtr& subpath)
{
    ExtensionMap::iterator rtes = routes.find(subpath);
    if (rtes != routes.end()) {
        for (RouteList::iterator rte = rtes->second.begin();
             rte != rtes->second.end(); ++rte)
        {
            if (routeq()(route, *rte)) {
                rtes->second.erase(rte);
                if (rtes->second.empty()) {
                    routes.erase(rtes);
                }
                return true;
            }
        }
    }
    return false;
}

void
Routing_module::add(RouteMap& routes, const RoutePtr& route)
{
    routes[route->id] = route;
}

void
Routing_module::add(RoutesMap& routes, const RoutePtr& route)
{
    RoutesMap::iterator rtes = routes.find(route->id);
    if (rtes == routes.end()) {
        routes[route->id] = RouteList(1, route);
    } else {
        uint32_t len = route->path.size();
        for (RouteList::iterator rte = rtes->second.begin();
             rte != rtes->second.end(); ++rte)
        {
            if ((*rte)->path.size() > len) {
                rtes->second.insert(rte, route);
                return;
            }
        }
        rtes->second.push_back(route);
    }
}

void
Routing_module::add(ExtensionMap& routes, const RoutePtr& route,
                    const RoutePtr& subpath)
{
    ExtensionMap::iterator rtes = routes.find(subpath);
    if (rtes == routes.end()) {
        routes[subpath] = RouteList(1, route);
    } else {
        rtes->second.push_back(route);
    }
}

// Methods handling a Flow_in_event, setting up the route to permit the flow
// all the way to its destination without having to go up to the controller for
// a permission check.

#define CHECK_OF_ERR(error, dp)                                         \
    if (error) {                                                        \
        if (error == EAGAIN) {                                          \
            VLOG_DBG(lg, "Add flow entry to dp:%"PRIx64" failed with EAGAIN.", \
                     dp.as_host());                                     \
        } else {                                                        \
            VLOG_ERR(lg, "Add flow entry to dp:%"PRIx64" failed with %d:%s.", \
                     dp.as_host(), error, strerror(error));             \
        }                                                               \
        os.str("");                                                     \
        return false;                                                   \
    }

void
Routing_module::init_openflow(uint16_t actions_len)
{
    len_flow_actions = actions_len;
    raw_of.reset(new uint8_t[sizeof(*ofm) + len_flow_actions]);
    ofm = (ofp_flow_mod*) raw_of.get();

    ofm->header.version = OFP_VERSION;
    ofm->header.type = OFPT_FLOW_MOD;
    ofm->header.xid = openflow_pack::get_xid();

    ofm->match.wildcards = 0;
    memset(&ofm->match.pad1, 0, sizeof(ofm->match.pad1));
    memset(&ofm->match.pad2, 0, sizeof(ofm->match.pad2));
    ofm->cookie = 0;
    ofm->command = htons(OFPFC_ADD);
    ofm->priority = htons(OFP_DEFAULT_PRIORITY);
    ofm->flags = htons(ofd_flow_mod_flags());
}

void
Routing_module::check_openflow(uint16_t actions_len)
{
    if (actions_len <= len_flow_actions) {
        return;
    } else if (raw_of == NULL) {
        init_openflow(actions_len);
        return;
    }

    actions_len *= 2;

    boost::shared_array<uint8_t> old_of = raw_of;
    raw_of.reset(new uint8_t[sizeof(*ofm) + actions_len]);
    ofm = (ofp_flow_mod*) raw_of.get();
    memcpy(ofm, old_of.get(), sizeof(*ofm));
    len_flow_actions = actions_len;
}

void
Routing_module::set_openflow(const Flow& flow, uint32_t buffer_id, uint16_t timeout)
{
    if (len_flow_actions < max_output_action_len) {
        init_openflow(8 * max_output_action_len);
    }

    ofm->buffer_id = htonl(buffer_id);
    ofm->idle_timeout = htons(timeout);
    ofm->hard_timeout = htons(OFP_FLOW_PERMANENT);
    ofm->cookie = htonll(flow.hash_code());

    ofp_match& match = ofm->match;
    match.in_port = flow.in_port;
    memcpy(match.dl_src, flow.dl_src.octet, ethernetaddr::LEN);
    memcpy(match.dl_dst, flow.dl_dst.octet, ethernetaddr::LEN);
    match.dl_vlan = flow.dl_vlan;
    match.dl_vlan_pcp = flow.dl_vlan_pcp;
    match.dl_type = flow.dl_type;
    match.nw_src = flow.nw_src;
    match.nw_dst = flow.nw_dst;
    match.nw_tos = flow.nw_tos;
    match.nw_proto = flow.nw_proto;
    match.tp_src = flow.tp_src;
    match.tp_dst = flow.tp_dst;
}

bool
Routing_module::set_openflow_actions(const Buffer& actions,
                                     const datapathid& dp, uint16_t outport,
                                     bool check_nat, bool allow_mac_overwrite)
{
    check_openflow(actions.size() + max_output_action_len);
    memcpy(ofm->actions, actions.data(), actions.size());

    uint16_t packet_len = sizeof(*ofm) + actions.size();
    bool nat_overwritten;
    bool natted = set_action(((uint8_t*)ofm->actions) + actions.size(),
                             dp, outport, packet_len, check_nat, allow_mac_overwrite,
                             0, nat_overwritten);
    ofm->header.length = htons(packet_len);
    return natted;
}

void
Routing_module::modify_match(const Buffer& actions)
{
    const uint8_t *pos = actions.data();
    uint16_t ofs = 0;

    ofp_match& match = ofm->match;

    while (ofs < actions.size()) {
        const ofp_action_header *action = (ofp_action_header*) (pos + ofs);

        switch (ntohs(action->type)) {
        case OFPAT_OUTPUT:
        case OFPAT_VENDOR:
            break;
        case OFPAT_SET_VLAN_PCP:
            match.dl_vlan_pcp = ((ofp_action_vlan_pcp*)action)->vlan_pcp;
            break;
        case OFPAT_SET_VLAN_VID:
            match.dl_vlan = ((ofp_action_vlan_vid*)action)->vlan_vid;
            break;
        case OFPAT_STRIP_VLAN:
            match.dl_vlan = htons(OFP_VLAN_NONE);
            break;
        case OFPAT_SET_DL_SRC:
            memcpy(match.dl_src, ((ofp_action_dl_addr*)action)->dl_addr, ethernetaddr::LEN);
            break;
        case OFPAT_SET_DL_DST:
            memcpy(match.dl_dst, ((ofp_action_dl_addr*)action)->dl_addr, ethernetaddr::LEN);
            break;
        case OFPAT_SET_NW_SRC:
            match.nw_src = ((ofp_action_nw_addr*)action)->nw_addr;
            break;
        case OFPAT_SET_NW_DST:
            match.nw_dst = ((ofp_action_nw_addr*)action)->nw_addr;
            break;
        case OFPAT_SET_TP_SRC:
            match.tp_src = ((ofp_action_tp_port*)action)->tp_port;
            break;
        case OFPAT_SET_TP_DST:
            match.tp_dst = ((ofp_action_tp_port*)action)->tp_port;
            break;
        default:
            VLOG_WARN(lg, "Routing_module doesn't recognize Openflow action %"PRIu16".",
                      ntohs(action->type));
        }
        ofs += ntohs(action->len);
    }
}

bool
Routing_module::setup_flow(const Flow& flow, const datapathid& dp,
                           uint16_t outport, uint32_t bid, const Buffer& buf,
                           uint16_t flow_timeout, const Buffer& actions,
                           bool check_nat,
                           const GroupList *sdladdr_groups,
                           const GroupList *snwaddr_groups,
                           const GroupList *ddladdr_groups,
                           const GroupList *dnwaddr_groups)
{
    uint32_t inport = ntohs(flow.in_port);
    uint16_t actions_len = actions.size();
    uint64_t overwrite = 0;
    bool nat_overwritten = false;

    set_openflow(flow, bid, flow_timeout);
    check_openflow(actions_len + max_output_action_len);
    memcpy(ofm->actions, actions.data(), actions.size());

    if (!check_nat) {
        set_action((uint8_t*)ofm->actions + actions_len, dp, outport,
                   actions_len, false, false, 0, nat_overwritten);
    } else {
        nat->get_nat_locations(&flow, sdladdr_groups, snwaddr_groups,
                               ddladdr_groups, dnwaddr_groups, nat_flow);
        bool allow_overwrite = !flow.dl_dst.is_multicast();
        if (outport == OFPP_FLOOD) {
            uint64_t orig_dl = flow.dl_dst.hb_long();
            const Topology::DpInfo& dpinfo = topology->get_dpinfo(dp);
            check_openflow((dpinfo.ports.size() * max_output_action_len) + actions_len);
            for (Topology::PortVector::const_iterator p_iter = dpinfo.ports.begin();
                 p_iter != dpinfo.ports.end(); ++p_iter)
            {
                if (p_iter->port_no != inport && p_iter->port_no < OFPP_MAX) {
                    set_action(((uint8_t*)ofm->actions) + actions_len, dp,
                               p_iter->port_no, actions_len, true,
                               allow_overwrite, overwrite,
                               nat_overwritten);
                    overwrite = nat_overwritten ? orig_dl : 0;
                }
            }
            if (actions_len == actions.size()) {
                VLOG_ERR(lg, "No outports to set flow actions with.");
                return false;
            }
        } else {
            set_action((uint8_t*)ofm->actions + actions_len, dp, outport,
                       actions_len, true, allow_overwrite, 0, nat_overwritten);
        }
    }
    ofm->header.length = htons(sizeof(*ofm) + actions_len);
    int ret = send_openflow_command(dp, &ofm->header, false);
    bool buf_sent = false;
    if (!ret && bid == UINT32_MAX) {
        buf_sent = true;
        ret = send_openflow_packet(dp, buf, ofm->actions,
                                   actions_len, inport, false);
    }

    if (ret) {
        if (ret == EAGAIN) {
            VLOG_DBG(lg, "Send openflow %s to dp:%"PRIx64" failed with EAGAIN.",
                     buf_sent ? "packet" : "command", dp.as_host());
        } else {
            VLOG_ERR(lg, "Send openflow %s to dp:%"PRIx64" failed with %d:%s.",
                     buf_sent ? "packet" : "command", dp.as_host(), ret, strerror(ret));
        }
        return false;
    }

    return true;
}

bool
Routing_module::setup_route(const Flow& flow, const Route& route,
                            uint16_t ap_inport, uint16_t ap_outport,
                            uint16_t flow_timeout, const ActionList& actions,
                            bool check_nat,
                            const GroupList *sdladdr_groups,
                            const GroupList *snwaddr_groups,
                            const GroupList *ddladdr_groups,
                            const GroupList *dnwaddr_groups)
{
    if (lg.is_dbg_enabled()) {
        os << flow;
        VLOG_DBG(lg, "Route: %s", os.str().c_str());
        os.str("");
        os << std::hex;
    }

    bool actions_def = !actions.empty();
    if (actions_def && actions.size() != route.path.size() + 1) {
        VLOG_ERR(lg, "Invalid length of ActionList, not enforcing.");
        actions_def = false;
    }

    set_openflow(flow, UINT32_MAX, flow_timeout);

    std::list<Link>::const_iterator link = route.path.begin();
    ActionList::const_iterator action = actions.begin();

    datapathid dp = route.id.src;
    uint16_t outport, inport = ap_inport;
    bool nat_match = false;
    if (check_nat) {
        nat->get_nat_locations(&flow, sdladdr_groups, snwaddr_groups,
                               ddladdr_groups, dnwaddr_groups, nat_flow);
    }
    while (true) {
        if (link == route.path.end()) {
            outport = ap_outport;
        } else {
            outport = link->outport;
        }

        if (inport == outport) {
            VLOG_WARN(lg, "Entry on dp:%"PRIx64" routes out inport:%"PRIu16".",
                      dp.as_host(), inport);
        }

        bool allow_overwrite = !flow.dl_dst.is_multicast();
        if (actions_def) {
            nat_match = set_openflow_actions(*action, dp, outport, check_nat,
                                             allow_overwrite);
        } else {
            uint16_t packet_len = sizeof(*ofm);
            bool nat_overwritten;
            nat_match = set_action((uint8_t*)ofm->actions, dp, outport, packet_len, check_nat,
                                   allow_overwrite, 0, nat_overwritten);
            ofm->header.length = htons(packet_len);
        }
        ofm->match.in_port = htons(inport);

        int err = send_openflow_command(dp, &ofm->header, false);
        CHECK_OF_ERR(err, dp);

        if (lg.is_dbg_enabled()) {
            os << inport << ":" << dp.as_host() << ':' << outport;
        }

        if (link == route.path.end() || nat_match) {
            break;
        }

        if (actions_def) {
            modify_match(*action);
            ++action;
        }

        dp = link->dst;
        inport = link->inport;
        ++link;
        if (lg.is_dbg_enabled()) {
            os << " --> ";
        }
    }

    if (lg.is_dbg_enabled()) {
        if (nat_match) {
            os << " NAT";
        }
        VLOG_DBG(lg, "%s", os.str().c_str());
        os.str("");
    }
    return true;
}

// return true if nat-ed, else false
bool
Routing_module::set_action(uint8_t *action, const datapathid& dp, uint16_t port,
                           uint16_t& actions_len, bool check_nat, bool allow_mac_overwrite,
                           uint64_t overwrite_mac, bool& nat_overwritten)
{
    uint64_t nat_dladdr = 0;
    bool natted = check_nat
        && nat->nat_location(dp.as_host() + (((uint64_t)port) << 48),
                             nat_dladdr, nat_flow);
    nat_overwritten = false;
    if (natted && allow_mac_overwrite && nat_dladdr != 0) {
        nat_overwritten = true;
        overwrite_mac = nat_dladdr;
    }
    if (overwrite_mac != 0) {
        ofp_action_dl_addr& dlset = *((ofp_action_dl_addr*)action);
        memset(&dlset, 0, sizeof(ofp_action_dl_addr));
        dlset.type = htons(OFPAT_SET_DL_DST);
        dlset.len = htons(sizeof(ofp_action_dl_addr));
        memcpy(dlset.dl_addr, ethernetaddr(overwrite_mac).octet, ethernetaddr::LEN);
        actions_len += sizeof(ofp_action_dl_addr);
        action += sizeof(ofp_action_dl_addr);
    }
    if (natted) {
        nx_action_snat& snat = *((nx_action_snat*)action);
        memset(&snat, 0, sizeof(nx_action_snat));
        snat.type = htons(OFPAT_VENDOR);
        snat.len = htons(sizeof(snat));
        snat.vendor = htonl(NX_VENDOR_ID);
        snat.subtype = htons(NXAST_SNAT);
        snat.port = htons(port);
        actions_len += sizeof(nx_action_snat);
        return true;
    }
    ofp_action_output& output = *((ofp_action_output*)action);
    memset(&output, 0, sizeof(ofp_action_output));
    output.type = htons(OFPAT_OUTPUT);
    output.len = htons(sizeof(output));
    output.max_len = 0;
    output.port = htons(port);
    actions_len += sizeof(ofp_action_output);
    return false;
}

bool
Routing_module::send_packet(const datapathid& dp, uint16_t inport, uint16_t outport,
                            uint32_t bid, const Buffer& buf,
                            const Buffer& actions,
                            bool check_nat, const Flow& flow,
                            const GroupList *sdladdr_groups,
                            const GroupList *snwaddr_groups,
                            const GroupList *ddladdr_groups,
                            const GroupList *dnwaddr_groups)
{
    uint16_t actions_len = actions.size();
    uint64_t overwrite = 0;
    bool nat_overwritten = false;

    check_openflow(actions_len + max_output_action_len);
    memcpy(ofm->actions, actions.data(), actions.size());

    if (!check_nat) {
        set_action(((uint8_t*)ofm->actions) + actions_len, dp, outport,
                   actions_len, false, false, 0, nat_overwritten);
    } else {
        nat->get_nat_locations(&flow, sdladdr_groups, snwaddr_groups,
                               ddladdr_groups, dnwaddr_groups, nat_flow);
        bool allow_overwrite = !flow.dl_dst.is_multicast();
        if (outport == OFPP_FLOOD) {
            const Topology::DpInfo& dpinfo = topology->get_dpinfo(dp);
            check_openflow(actions_len + (dpinfo.ports.size() * max_output_action_len));
            uint64_t orig_dl = flow.dl_dst.hb_long();
            for (Topology::PortVector::const_iterator p_iter = dpinfo.ports.begin();
                 p_iter != dpinfo.ports.end(); ++p_iter)
            {
                if (p_iter->port_no != inport && p_iter->port_no < OFPP_MAX) {
                    set_action(((uint8_t*)ofm->actions) + actions_len, dp,
                               p_iter->port_no, actions_len, true,
                               allow_overwrite, overwrite, nat_overwritten);
                    overwrite = nat_overwritten ? orig_dl : 0;
                }
            }
            if (actions_len == actions.size()) {
                VLOG_ERR(lg, "No outports to set packet actions with.");
                return false;
            }
        } else {
            set_action(((uint8_t*)ofm->actions) + actions_len, dp, outport,
                       actions_len, true, allow_overwrite, 0, nat_overwritten);
        }
    }

    int ret;
    if (bid == UINT32_MAX) {
        ret = send_openflow_packet(dp, buf, ofm->actions,
                                   actions_len, inport, false);
    } else {
        ret = send_openflow_packet(dp, bid, ofm->actions,
                                   actions_len, inport, false);
    }

    if (ret) {
        if (ret == EAGAIN) {
            VLOG_DBG(lg, "Send packet to dp:%"PRIx64" failed with EAGAIN.",
                     dp.as_host());
        } else {
            VLOG_ERR(lg, "Send packet to dp:%"PRIx64" failed with %d:%s.",
                     dp.as_host(), ret, strerror(ret));
        }
        return false;
    }

    return true;
}

}
}

REGISTER_COMPONENT(vigil::container::Simple_component_factory
                   <vigil::applications::Routing_module>,
                   vigil::applications::Routing_module);

ext: cc

max_stars_repo_name: ayjazz/OESS

max_issues_repo_name: ayjazz/OESS

max_forks_repo_name: ayjazz/OESS


text:
/*

Copyright (c) 2015, Project OSRM contributors
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 "data_structures/original_edge_data.hpp"
#include "data_structures/range_table.hpp"
#include "data_structures/query_edge.hpp"
#include "data_structures/query_node.hpp"
#include "data_structures/shared_memory_factory.hpp"
#include "data_structures/shared_memory_vector_wrapper.hpp"
#include "data_structures/static_graph.hpp"
#include "data_structures/static_rtree.hpp"
#include "data_structures/travel_mode.hpp"
#include "data_structures/turn_instructions.hpp"
#include "server/data_structures/datafacade_base.hpp"
#include "server/data_structures/shared_datatype.hpp"
#include "server/data_structures/shared_barriers.hpp"
#include "util/datastore_options.hpp"
#include "util/simple_logger.hpp"
#include "util/osrm_exception.hpp"
#include "util/fingerprint.hpp"
#include "typedefs.h"

#include <osrm/coordinate.hpp>
#include <osrm/server_paths.hpp>

using RTreeLeaf = BaseDataFacade<QueryEdge::EdgeData>::RTreeLeaf;
using RTreeNode = StaticRTree<RTreeLeaf, ShM<FixedPointCoordinate, true>::vector, true>::TreeNode;
using QueryGraph = StaticGraph<QueryEdge::EdgeData>;

#ifdef __linux__
#include <sys/mman.h>
#endif

#include <boost/filesystem/fstream.hpp>
#include <boost/iostreams/seek.hpp>

#include <cstdint>

#include <fstream>
#include <string>
#include <new>

// delete a shared memory region. report warning if it could not be deleted
void delete_region(const SharedDataType region)
{
    if (SharedMemory::RegionExists(region) && !SharedMemory::Remove(region))
    {
        const std::string name = [&]
        {
            switch (region)
            {
            case CURRENT_REGIONS:
                return "CURRENT_REGIONS";
            case LAYOUT_1:
                return "LAYOUT_1";
            case DATA_1:
                return "DATA_1";
            case LAYOUT_2:
                return "LAYOUT_2";
            case DATA_2:
                return "DATA_2";
            case LAYOUT_NONE:
                return "LAYOUT_NONE";
            default: // DATA_NONE:
                return "DATA_NONE";
            }
        }();

        SimpleLogger().Write(logWARNING) << "could not delete shared memory region " << name;
    }
}

int main(const int argc, const char *argv[]) try
{
    LogPolicy::GetInstance().Unmute();
    SharedBarriers barrier;

#ifdef __linux__
    // try to disable swapping on Linux
    const bool lock_flags = MCL_CURRENT | MCL_FUTURE;
    if (-1 == mlockall(lock_flags))
    {
        SimpleLogger().Write(logWARNING) << "Process " << argv[0] << " could not request RAM lock";
    }
#endif

    try
    {
        boost::interprocess::scoped_lock<boost::interprocess::named_mutex> pending_lock(
            barrier.pending_update_mutex);
    }
    catch (...)
    {
        // hard unlock in case of any exception.
        barrier.pending_update_mutex.unlock();
    }

    SimpleLogger().Write(logDEBUG) << "Checking input parameters";

    ServerPaths server_paths;
    if (!GenerateDataStoreOptions(argc, argv, server_paths))
    {
        return EXIT_SUCCESS;
    }

    if (server_paths.find("hsgrdata") == server_paths.end())
    {
        throw osrm::exception("no hsgr file found");
    }
    if (server_paths.find("ramindex") == server_paths.end())
    {
        throw osrm::exception("no ram index file found");
    }
    if (server_paths.find("fileindex") == server_paths.end())
    {
        throw osrm::exception("no leaf index file found");
    }
    if (server_paths.find("nodesdata") == server_paths.end())
    {
        throw osrm::exception("no nodes file found");
    }
    if (server_paths.find("edgesdata") == server_paths.end())
    {
        throw osrm::exception("no edges file found");
    }
    if (server_paths.find("namesdata") == server_paths.end())
    {
        throw osrm::exception("no names file found");
    }
    if (server_paths.find("geometry") == server_paths.end())
    {
        throw osrm::exception("no geometry file found");
    }
    if (server_paths.find("core") == server_paths.end())
    {
        throw osrm::exception("no core file found");
    }

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

    // determine segment to use
    bool segment2_in_use = SharedMemory::RegionExists(LAYOUT_2);
    const SharedDataType layout_region = [&]
    {
        return segment2_in_use ? LAYOUT_1 : LAYOUT_2;
    }();
    const SharedDataType data_region = [&]
    {
        return segment2_in_use ? DATA_1 : DATA_2;
    }();
    const SharedDataType previous_layout_region = [&]
    {
        return segment2_in_use ? LAYOUT_2 : LAYOUT_1;
    }();
    const SharedDataType previous_data_region = [&]
    {
        return segment2_in_use ? DATA_2 : DATA_1;
    }();

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

    shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::FILE_INDEX_PATH,
                                          file_index_path.length() + 1);

    // collect number of elements to store in shared memory object
    SimpleLogger().Write() << "load names from: " << names_data_path;
    // number of entries in name index
    boost::filesystem::ifstream name_stream(names_data_path, std::ios::binary);
    unsigned name_blocks = 0;
    name_stream.read((char *)&name_blocks, sizeof(unsigned));
    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::NAME_OFFSETS, name_blocks);
    shared_layout_ptr->SetBlockSize<typename RangeTable<16, true>::BlockT>(
        SharedDataLayout::NAME_BLOCKS, name_blocks);
    SimpleLogger().Write() << "name offsets size: " << name_blocks;
    BOOST_ASSERT_MSG(0 != name_blocks, "name file broken");

    unsigned number_of_chars = 0;
    name_stream.read((char *)&number_of_chars, sizeof(unsigned));
    shared_layout_ptr->SetBlockSize<char>(SharedDataLayout::NAME_CHAR_LIST, number_of_chars);

    // 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));

    // note: settings this all to the same size is correct, we extract them from the same struct
    shared_layout_ptr->SetBlockSize<NodeID>(SharedDataLayout::VIA_NODE_LIST,
                                            number_of_original_edges);
    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::NAME_ID_LIST,
                                              number_of_original_edges);
    shared_layout_ptr->SetBlockSize<TravelMode>(SharedDataLayout::TRAVEL_MODE,
                                                number_of_original_edges);
    shared_layout_ptr->SetBlockSize<TurnInstruction>(SharedDataLayout::TURN_INSTRUCTION,
                                                     number_of_original_edges);
    // note: there are 32 geometry indicators in one unsigned block
    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_INDICATORS,
                                              number_of_original_edges);

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

    FingerPrint fingerprint_valid = FingerPrint::GetValid();
    FingerPrint fingerprint_loaded;
    hsgr_input_stream.read((char *)&fingerprint_loaded, sizeof(FingerPrint));
    if (fingerprint_loaded.TestGraphUtil(fingerprint_valid))
    {
        SimpleLogger().Write(logDEBUG) << "Fingerprint checked out ok";
    }
    else
    {
        SimpleLogger().Write(logWARNING) << ".hsgr was prepared with different build. "
                                            "Reprocess to get rid of this warning.";
    }

    // load checksum
    unsigned checksum = 0;
    hsgr_input_stream.read((char *)&checksum, sizeof(unsigned));
    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::HSGR_CHECKSUM, 1);
    // 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->SetBlockSize<QueryGraph::NodeArrayEntry>(SharedDataLayout::GRAPH_NODE_LIST,
                                                                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->SetBlockSize<QueryGraph::EdgeArrayEntry>(SharedDataLayout::GRAPH_EDGE_LIST,
                                                                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->SetBlockSize<RTreeNode>(SharedDataLayout::R_SEARCH_TREE, tree_size);

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

    // load core marker size
    boost::filesystem::ifstream core_marker_file(core_marker_path, std::ios::binary);

    uint32_t number_of_core_markers = 0;
    core_marker_file.read((char *)&number_of_core_markers, sizeof(uint32_t));
    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::CORE_MARKER,
                                              number_of_core_markers);

    // 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->SetBlockSize<FixedPointCoordinate>(SharedDataLayout::COORDINATE_LIST,
                                                          coordinate_list_size);

    // load geometries sizes
    std::ifstream geometry_input_stream(geometries_data_path.string().c_str(), std::ios::binary);
    unsigned number_of_geometries_indices = 0;
    unsigned number_of_compressed_geometries = 0;

    geometry_input_stream.read((char *)&number_of_geometries_indices, sizeof(unsigned));
    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_INDEX,
                                              number_of_geometries_indices);
    boost::iostreams::seek(geometry_input_stream, number_of_geometries_indices * sizeof(unsigned),
                           BOOST_IOS::cur);
    geometry_input_stream.read((char *)&number_of_compressed_geometries, sizeof(unsigned));
    shared_layout_ptr->SetBlockSize<unsigned>(SharedDataLayout::GEOMETRIES_LIST,
                                              number_of_compressed_geometries);
    // allocate shared memory block
    SimpleLogger().Write() << "allocating shared memory of " << shared_layout_ptr->GetSizeOfLayout()
                           << " bytes";
    SharedMemory *shared_memory =
        SharedMemoryFactory::Get(data_region, shared_layout_ptr->GetSizeOfLayout());
    char *shared_memory_ptr = static_cast<char *>(shared_memory->Ptr());

    // read actual data into shared memory object //

    // hsgr checksum
    unsigned *checksum_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::HSGR_CHECKSUM);
    *checksum_ptr = checksum;

    // ram index file name
    char *file_index_path_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
        shared_memory_ptr, SharedDataLayout::FILE_INDEX_PATH);
    // make sure we have 0 ending
    std::fill(file_index_path_ptr,
              file_index_path_ptr +
                  shared_layout_ptr->GetBlockSize(SharedDataLayout::FILE_INDEX_PATH),
              0);
    std::copy(file_index_path.begin(), file_index_path.end(), file_index_path_ptr);

    // Loading street names
    unsigned *name_offsets_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::NAME_OFFSETS);
    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_OFFSETS) > 0)
    {
        name_stream.read((char *)name_offsets_ptr,
                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_OFFSETS));
    }

    unsigned *name_blocks_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::NAME_BLOCKS);
    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_BLOCKS) > 0)
    {
        name_stream.read((char *)name_blocks_ptr,
                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_BLOCKS));
    }

    char *name_char_ptr = shared_layout_ptr->GetBlockPtr<char, true>(
        shared_memory_ptr, SharedDataLayout::NAME_CHAR_LIST);
    unsigned temp_length;
    name_stream.read((char *)&temp_length, sizeof(unsigned));

    BOOST_ASSERT_MSG(temp_length ==
                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST),
                     "Name file corrupted!");

    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST) > 0)
    {
        name_stream.read(name_char_ptr,
                         shared_layout_ptr->GetBlockSize(SharedDataLayout::NAME_CHAR_LIST));
    }

    name_stream.close();

    // load original edge information
    NodeID *via_node_ptr = shared_layout_ptr->GetBlockPtr<NodeID, true>(
        shared_memory_ptr, SharedDataLayout::VIA_NODE_LIST);

    unsigned *name_id_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::NAME_ID_LIST);

    TravelMode *travel_mode_ptr = shared_layout_ptr->GetBlockPtr<TravelMode, true>(
        shared_memory_ptr, SharedDataLayout::TRAVEL_MODE);

    TurnInstruction *turn_instructions_ptr = shared_layout_ptr->GetBlockPtr<TurnInstruction, true>(
        shared_memory_ptr, SharedDataLayout::TURN_INSTRUCTION);

    unsigned *geometries_indicator_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::GEOMETRIES_INDICATORS);

    OriginalEdgeData current_edge_data;
    for (unsigned i = 0; i < number_of_original_edges; ++i)
    {
        edges_input_stream.read((char *)&(current_edge_data), sizeof(OriginalEdgeData));
        via_node_ptr[i] = current_edge_data.via_node;
        name_id_ptr[i] = current_edge_data.name_id;
        travel_mode_ptr[i] = current_edge_data.travel_mode;
        turn_instructions_ptr[i] = current_edge_data.turn_instruction;

        const unsigned bucket = i / 32;
        const unsigned offset = i % 32;
        const unsigned value = [&]
        {
            unsigned return_value = 0;
            if (0 != offset)
            {
                return_value = geometries_indicator_ptr[bucket];
            }
            return return_value;
        }();
        if (current_edge_data.compressed_geometry)
        {
            geometries_indicator_ptr[bucket] = (value | (1 << offset));
        }
    }
    edges_input_stream.close();

    // load compressed geometry
    unsigned temporary_value;
    unsigned *geometries_index_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::GEOMETRIES_INDEX);
    geometry_input_stream.seekg(0, geometry_input_stream.beg);
    geometry_input_stream.read((char *)&temporary_value, sizeof(unsigned));
    BOOST_ASSERT(temporary_value ==
                 shared_layout_ptr->num_entries[SharedDataLayout::GEOMETRIES_INDEX]);

    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_INDEX) > 0)
    {
        geometry_input_stream.read(
            (char *)geometries_index_ptr,
            shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_INDEX));
    }
    unsigned *geometries_list_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::GEOMETRIES_LIST);

    geometry_input_stream.read((char *)&temporary_value, sizeof(unsigned));
    BOOST_ASSERT(temporary_value ==
                 shared_layout_ptr->num_entries[SharedDataLayout::GEOMETRIES_LIST]);

    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_LIST) > 0)
    {
        geometry_input_stream.read(
            (char *)geometries_list_ptr,
            shared_layout_ptr->GetBlockSize(SharedDataLayout::GEOMETRIES_LIST));
    }

    // Loading list of coordinates
    FixedPointCoordinate *coordinates_ptr =
        shared_layout_ptr->GetBlockPtr<FixedPointCoordinate, true>(
            shared_memory_ptr, SharedDataLayout::COORDINATE_LIST);

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

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

    // store search tree portion of rtree
    char *rtree_ptr = shared_layout_ptr->GetBlockPtr<char, true>(shared_memory_ptr,
                                                                 SharedDataLayout::R_SEARCH_TREE);

    if (tree_size > 0)
    {
        tree_node_file.read(rtree_ptr, sizeof(RTreeNode) * tree_size);
    }
    tree_node_file.close();

    // load core markers
    std::vector<char> unpacked_core_markers(number_of_core_markers);
    core_marker_file.read((char *)unpacked_core_markers.data(),
                          sizeof(char) * number_of_core_markers);

    unsigned *core_marker_ptr = shared_layout_ptr->GetBlockPtr<unsigned, true>(
        shared_memory_ptr, SharedDataLayout::CORE_MARKER);

    for (auto i = 0u; i < number_of_core_markers; ++i)
    {
        BOOST_ASSERT(unpacked_core_markers[i] == 0 || unpacked_core_markers[i] == 1);

        if (unpacked_core_markers[i] == 1)
        {
            const unsigned bucket = i / 32;
            const unsigned offset = i % 32;
            const unsigned value = [&]
            {
                unsigned return_value = 0;
                if (0 != offset)
                {
                    return_value = core_marker_ptr[bucket];
                }
                return return_value;
            }();

            core_marker_ptr[bucket] = (value | (1 << offset));
        }
    }

    // load the nodes of the search graph
    QueryGraph::NodeArrayEntry *graph_node_list_ptr =
        shared_layout_ptr->GetBlockPtr<QueryGraph::NodeArrayEntry, true>(
            shared_memory_ptr, SharedDataLayout::GRAPH_NODE_LIST);
    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_NODE_LIST) > 0)
    {
        hsgr_input_stream.read((char *)graph_node_list_ptr,
                               shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_NODE_LIST));
    }

    // load the edges of the search graph
    QueryGraph::EdgeArrayEntry *graph_edge_list_ptr =
        shared_layout_ptr->GetBlockPtr<QueryGraph::EdgeArrayEntry, true>(
            shared_memory_ptr, SharedDataLayout::GRAPH_EDGE_LIST);
    if (shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_EDGE_LIST) > 0)
    {
        hsgr_input_stream.read((char *)graph_edge_list_ptr,
                               shared_layout_ptr->GetBlockSize(SharedDataLayout::GRAPH_EDGE_LIST));
    }
    hsgr_input_stream.close();

    // acquire lock
    SharedMemory *data_type_memory =
        SharedMemoryFactory::Get(CURRENT_REGIONS, sizeof(SharedDataTimestamp), true, false);
    SharedDataTimestamp *data_timestamp_ptr =
        static_cast<SharedDataTimestamp *>(data_type_memory->Ptr());

    boost::interprocess::scoped_lock<boost::interprocess::named_mutex> query_lock(
        barrier.query_mutex);

    // notify all processes that were waiting for this condition
    if (0 < barrier.number_of_queries)
    {
        barrier.no_running_queries_condition.wait(query_lock);
    }

    data_timestamp_ptr->layout = layout_region;
    data_timestamp_ptr->data = data_region;
    data_timestamp_ptr->timestamp += 1;
    delete_region(previous_data_region);
    delete_region(previous_layout_region);
    SimpleLogger().Write() << "all data loaded";

    shared_layout_ptr->PrintInformation();
}
catch (const std::bad_alloc &e)
{
    SimpleLogger().Write(logWARNING) << "[exception] " << e.what();
    SimpleLogger().Write(logWARNING) << "Please provide more memory or disable locking the virtual "
                                        "address space (note: this makes OSRM swap, i.e. slow)";
    return EXIT_FAILURE;
}
catch (const std::exception &e)
{
    SimpleLogger().Write(logWARNING) << "caught exception: " << e.what();
}

max_stars_repo_name: Mapotempo/osrm-backend

max_issues_repo_name: Mapotempo/osrm-backend

max_forks_repo_name: Mapotempo/osrm-backend