text:
#ifndef UTILITIES_H
#define UTILITIES_H

#include <array>
#include <atomic>
#include <cstdlib>
#include <fstream>
#include <random>
#include <stdexcept>
#include <string>
#include <sys/stat.h>

#include <boost/filesystem.hpp>

#include "reportingUtils.hpp"

#include <seqan/modifier.h>
#include <seqan/sequence.h>
#include <seqan/bam_io.h>

#include "CMakeConfig.h"

//https://stackoverflow.com/questions/3599160/how-to-suppress-unused-parameter-warnings-in-c
#ifdef __GNUC__
#  define UNUSED(x) UNUSED_ ## x __attribute__((__unused__))
#else
#  define UNUSED(x) UNUSED_ ## x
#endif

namespace reseq{
	// Type definitions
	typedef int8_t intQualDiff; // Difference of quality values

	typedef uint8_t uintInDelType; // InDel Type (0,1)
	typedef uint8_t uintPercent; // Percent values
	typedef uint8_t uintQual; // Quality values
	typedef uint8_t uintTempSeq; // Template segment, strand (0,1)

	typedef int16_t uintPercentShift; // Percent values with direction information for shifts

	typedef uint16_t uintAdapterId; // Adapter id
	typedef uint16_t uintAlleleId; // Allele number, id, etc.
	typedef uint16_t uintBaseCall; // Base call in case it is converted from seqan::Dna5(or other) to int
	typedef uint16_t uintDupCount; // Count of fragments at a given site
	typedef uint16_t uintErrorCount; // Count of errors that occurred during execution
	typedef uint16_t uintInDelTypePrint; // InDel Type (0,1) for printing (so they are not printed as characters)
	typedef uint16_t uintMarginId; // Id or size for probability margin or dimension
	typedef uint16_t uintNumThreads; // Id for probability margin
	typedef uint16_t uintPercentPrint; // Percent values for printing (so they are not printed as characters)
	typedef uint16_t uintQualPrint; // Quality values for printing (so they are not printed as characters)
	typedef uint16_t uintReadLen; // Position on read, length of sequence
	typedef uint16_t uintSurBlockId; // Surrounding block number, id, etc.
	typedef uint16_t uintSurPos; // Position in surrounding
	typedef uint16_t uintTempSeqPrint; // Template segment, strand (0,1) for printing (so they are not printed as characters)
	typedef uint16_t uintTile; // Tile encoding (e.g. 2308)
	typedef uint16_t uintTileId; // Tile ids

	typedef int32_t intSeqShift; // Sequence length with direction information for shifts
	typedef int32_t intVariantId; // Variant id, number, etc. (id can be invalid < 0)

	typedef uint32_t uintCovCount; // Count for position coverage
	typedef uint32_t uintMatrixIndex; // Index or size for probability matrix
	typedef uint32_t uintNumFits; // Number of fits, bias sum parameters(ref seq, insert length), iterations, parameters
	typedef uint32_t uintReadLenCalc; // Calculations on read length producing temporary larger values, like calculating the mean of something over all positions
	typedef uint32_t uintRefSeqBin; // Reference sequence bin/block id, number, etc.
	typedef uint32_t uintRefSeqId; // Reference sequence id, number, etc.
	typedef uint32_t uintSeqLen; // Position on reference sequence, length of sequence, distance

	typedef int64_t intExtSurrounding; // Extended surrounding value to combine blocks, etc.
	typedef int64_t intFragCountShift; // General count of fragments/reads with direction information for shifts

	typedef uint64_t uintAlleleBitArray; // bit array for storing yes/no for 64 alleles
	typedef uint64_t uintFragCount; // General count of fragments/reads
	typedef uint64_t uintMatrixCount; // Count in probability matrix independent of origin (at least as big as uintFragCount, uintNucCount)
	typedef uint64_t uintNucCount; // General count of fragments/reads
	typedef uint64_t uintRefLenCalc; // Calculations with reference sequences producing temporary large values, like summing up length of reference sequences or getting a mean
	typedef uint64_t uintSeed; // Seed value for random number generator

#ifndef SWIG // This part is not needed for the python plotting and swig can't handle the seqan stuff
	namespace utilities{
		// Type definitions
		typedef seqan::String<seqan::CigarElement<>> CigarString;
		typedef seqan::ModView< seqan::FunctorComplement<seqan::Iupac> > ModComplementIupac;

		typedef const seqan::ModifiedString< seqan::ModifiedString<const seqan::Dna5String, seqan::ModComplementDna5>, seqan::ModReverse> ConstDna5StringReverseComplement;
		typedef const seqan::ModifiedString< seqan::ModifiedString<const seqan::DnaString, seqan::ModComplementDna>, seqan::ModReverse> ConstDnaStringReverseComplement;
		typedef const seqan::ModifiedString< seqan::ModifiedString<const seqan::IupacString, ModComplementIupac>, seqan::ModReverse> ConstIupacStringReverseComplement;

		typedef const seqan::ModifiedString<const seqan::Dna5String, seqan::ModComplementDna5> ComplementedConstDna5String;

		typedef const seqan::ModifiedString< const seqan::CharString, seqan::ModReverse>  ReversedConstCharString;
		typedef const seqan::ModifiedString< const CigarString, seqan::ModReverse> ReversedConstCigarString;

		// Functions needed in classes
		template<typename T> struct AtDummy{
			static T dummy_;
		};

		template<typename T> inline const T &at(const seqan::StringSet<T> &set, size_t n){
			if( length(set) > n ){
				return set[n];
			}
			else{
				printErr << "Called position " << n << ". StringSet size is only " << length(set) << "." << std::endl;
				throw std::out_of_range( "Accessing StringSet after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T> inline T &at(seqan::StringSet<T> &set, size_t n){
			if( length(set) > n ){
				return set[n];
			}
			else{
				printErr << "Called position " << n << ". StringSet size is only " << length(set) << "." << std::endl;
				throw std::out_of_range( "Accessing StringSet after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T> inline const T &at(const seqan::String<T>& string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". String size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing String after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T> inline T &at(seqan::String<T>& string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". String size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing String after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		// Do not return references for ModifiedString types, as they generate temporary values and it does not work and we don't want to change the underlying string in those cases anyways
		template<typename T, typename U> inline T at(const seqan::ModifiedString<const seqan::String<T>, U> &string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". ModifiedString size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing ModifiedString after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T, typename U> inline T at(const seqan::ModifiedString<seqan::String<T>, U> &string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". ModifiedString size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing ModifiedString after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T, typename U> inline T at(seqan::ModifiedString<seqan::String<T>, U> &string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". ModifiedString size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing ModifiedString after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T, typename U, typename V> inline T at(const seqan::ModifiedString< seqan::ModifiedString<const seqan::String<T>, U>, V> &string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". ModifiedString size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing ModifiedString after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T, typename U, typename V> inline T at(const seqan::ModifiedString< seqan::ModifiedString<seqan::String<T>, U>, V> &string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". ModifiedString size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing ModifiedString after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T, typename U, typename V> inline T at(seqan::ModifiedString< seqan::ModifiedString<seqan::String<T>, U>, V> &string, size_t n){
			if( length(string) > n ){
				return string[n];
			}
			else{
				printErr << "Called position " << n << ". ModifiedString size is only " << length(string) << "." << std::endl;
				throw std::out_of_range( "Accessing ModifiedString after last position" );
				return AtDummy<T>::dummy_;
			}
		}

		template<typename T, typename U> inline void SetToMax(T& current_max, const U potential_max){
			if(potential_max > current_max ){
				current_max = potential_max;
			}
		}

		template<typename T, typename U> inline void SetToMin(T& current_min, const U potential_min){
			if(potential_min < current_min ){
				current_min = potential_min;
			}
		}

		// Mini classes
		class Complement{
		public:
			static seqan::FunctorComplement<seqan::Dna5> Dna5;
			static seqan::FunctorComplement<seqan::Dna> Dna;
		};

		class DominantBase{
		public:
			static const uintReadLen kLastX = 5;

		private:
			seqan::Dna dom_base_;
			std::array<uintReadLen, 5> seq_content_;

			template<typename S> void FindDominant( const S &seq, uintSeqLen cur_pos ){
				// Get count of most appearing base
				uintSeqLen max_content(0);
				for( uintBaseCall nucleotide=4; nucleotide--; ){ // Leave out N for this
					SetToMax(max_content, seq_content_.at(nucleotide));
				}

				if(0 == max_content){
					// Only N's in kLastX
					if(length(seq) <= cur_pos || 4 == at(seq, cur_pos)){
						dom_base_ = 0; // We won't use this position anyways for the statistics so just make it an A
					}
					else{
						dom_base_ = at(seq, cur_pos); // Use cur base as all before where N's
					}
				}
				else{
					// Get the base that is closest to the current base which matches max_content (in case there are multiple bases with same number of appearances)
					uintSeqLen pos=cur_pos;
					while(max_content != seq_content_.at( at(seq, --pos) ));
					dom_base_ = at(seq, pos);
				}
			}

		public:
			DominantBase():
				dom_base_(0){
				seq_content_.fill(0);
			}

			DominantBase& operator=(const DominantBase& rhs){
				dom_base_ = rhs.dom_base_;
				seq_content_ = rhs.seq_content_;
				return *this;
			}

			seqan::Dna Get(){
				return dom_base_;
			}

			void Clear(){
				seq_content_.fill(0);
			}

			template<typename S> void Set( const S &seq, uintSeqLen cur_pos ){
				// Fill seq_content_
				for( uintSeqLen pos = (kLastX<cur_pos?cur_pos-kLastX:0); pos < cur_pos; ++pos){
					++seq_content_.at( at(seq, pos) );
				}

				FindDominant(seq, cur_pos);
			}

			template<typename U, typename S> void Update( U base, const S &seq, uintSeqLen last_pos ){
				// Add new base
				++seq_content_.at(base);
				// Remove old base if we reached kLastX bases, so that the sum of seq_content_ is actually kLastX
				if(kLastX <= last_pos){
					--seq_content_.at(at(seq, last_pos-kLastX));
				}

				FindDominant(seq, last_pos+1);
			}
		};

		class DominantBaseWithMemory{
		private:
			DominantBase dom_base_;
			seqan::Dna5String memory_;

		public:
			DominantBaseWithMemory(){
				reserve(memory_, DominantBase::kLastX+2);
			}

			DominantBaseWithMemory& operator=(const DominantBaseWithMemory& rhs){
				dom_base_ = rhs.dom_base_;
				memory_ = rhs.memory_;
				return *this;
			}

			seqan::Dna Get(){
				return dom_base_.Get();
			}

			void Clear(){
				dom_base_.Clear();
				clear(memory_);
			}

			template<typename S> void Set( const S &seq, uintSeqLen cur_pos ){
				resize( memory_, std::min(static_cast<uintSeqLen>(DominantBase::kLastX), cur_pos)+1 );
				for(auto mem_pos=length(memory_); mem_pos--; ){
					at(memory_, mem_pos) = at(seq, cur_pos + mem_pos + 1 - length(memory_));
				}
				dom_base_.Set( memory_, length(memory_)-1 );
			}

			// Update the current base not the last base like in DominantBase as we need to already store the current base
			template<typename U> void Update( U base ){
				if(length(memory_) > DominantBase::kLastX+1){
					erase(memory_, 0);
				}
				memory_ += base;

				if(1 < length(memory_)){
					// Only update when we have a last base.
					dom_base_.Update( at(memory_, length(memory_)-2), memory_, length(memory_)-2 );
				}
				else{
					// When we only added the current base we don't want to add a base, but still set the dominant base to the current base
					dom_base_.Set( memory_, 0 );
				}
			}
		};

		template<typename T> struct VectorAtomic{
			typename std::atomic<T> value_;

			VectorAtomic():
				value_(0){
			}

			VectorAtomic(const VectorAtomic &UNUSED(right)){
				throw std::runtime_error( "This function should never be called. Did you resize an object with a non-zero length?" );
			}

			operator T() const{ return value_; }

			template<typename U> inline VectorAtomic<T> &operator=(const U &value){
				value_ = value;
				return *this;
			}

			template<typename U> bool operator==(const U& comp) const{
				return value_ == comp;
			}

			inline VectorAtomic<T> &operator++(){
				++value_;
				return *this;
			}

			inline T operator++(int){
				return value_++;
			}

			template<typename U> inline T operator+=(const U &rhs){
				return value_ += rhs;
			}

			inline VectorAtomic<T> &operator--(){
				--value_;
				return *this;
			}

			inline T operator--(int){
				return value_--;
			}

			template<typename U> inline T operator-=(const U &rhs){
				return value_ -= rhs;
			}
		};
		template<typename T, typename U> bool operator==( U lhs, const VectorAtomic<T>& rhs){ // Compares the size_t value to the size
			return rhs == lhs;
		}

		// Functions
		template<typename T> void Acquire(std::vector<T> &receiver, std::vector<VectorAtomic<T>> &donator){
			receiver.resize(donator.size());
			for(auto i=donator.size(); i--; ){
				receiver.at(i) = donator.at(i);
			}
			donator.clear();
			donator.shrink_to_fit();
		}

		inline void CreateDir(const char *file){
			auto file_path = boost::filesystem::path(file).parent_path();
			if( !file_path.empty() ){
				boost::filesystem::create_directories(file_path);
			}
		}

		inline bool FileExists(const std::string &file){
			struct stat buffer;
			return (stat(file.c_str(), &buffer) == 0);
		}

		inline void DeleteFile(const char *file){
			boost::filesystem::remove(boost::filesystem::path(file));
		}

		inline bool GetReSeqDir(std::string &full_dir, const std::string dir, const std::string test_file){
			full_dir = std::string(PROJECT_SOURCE_DIR) + '/' + dir + '/';
			if(!FileExists(full_dir + test_file)){
				full_dir = std::string( std::getenv("CONDA_PREFIX") ) + "/etc/reseq/" + dir + '/';
				if(!FileExists(full_dir + test_file)){
					full_dir = std::string( std::getenv("RESEQ_FOLDER") ) + '/' + dir + '/';
					if(!FileExists(full_dir + test_file)){
						printErr << "Could not find " << dir << " folder. Searching at " << std::endl <<
								(std::string(PROJECT_SOURCE_DIR)+'/'+dir+'/') << std::endl <<
								("${CONDA_PREFIX}/etc/reseq/"+dir+'/') << std::endl <<
								("${RESEQ_FOLDER}/"+dir+'/') << std::endl;
						return false;
					}
				}
			}

			return true;
		}

		template<typename T> inline T Divide(T nom, T den){ // Calculates the division: nom/den with proper rounding to int
			return (nom+den/static_cast<T>(2))/den;
		}
		template<typename T> inline T Divide(const std::atomic<T> &nom, T den){ // Calculates the division: nom/den with proper rounding to int
			return (nom+den/static_cast<T>(2))/den;
		}
		template<typename T> inline T Divide(T nom, const std::atomic<T> &den){ // Calculates the division: nom/den with proper rounding to int
			return (nom+den/static_cast<T>(2))/den;
		}
		template<typename T> inline T Divide(const std::atomic<T> &nom, const std::atomic<T> &den){ // Calculates the division: nom/den with proper rounding to int
			return (nom+den/static_cast<T>(2))/den;
		}
		template<typename T> inline T DivideAndCeil(T nom, T den){ // Calculates the division: nom/den and always ceiling
			return (nom+den-static_cast<T>(1)) / den;
		}

		inline bool FileExists(const char *file_name){
			std::ifstream ifile(file_name);
			return static_cast<bool>(ifile);
		}

		template<typename T> inline const T &getConst(T &object){
			return object;
		}

		inline bool IsN(seqan::Dna5 base);
		template<typename T> inline bool HasNTemplate(const T& sequence){
			for( auto base : sequence ){
				if( IsN(base) ){
					return true;
				}
			}

			return false;
		}

		// Specify possible options so we do not accidentally call function with seqan::CharString etc., which results in wrong output
		inline bool HasN(const seqan::Dna5String& sequence){
			return HasNTemplate(sequence);
		}
		inline bool HasN(const seqan::Dna5StringReverseComplement& sequence){
			return HasNTemplate(sequence);
		}
		inline bool HasN(const ConstDna5StringReverseComplement& sequence){
			return HasNTemplate(sequence);
		}

		constexpr uint64_t IntPow(uint16_t base, uint16_t power){
			uint64_t result = 1;
			for(auto i=power; i--; ){
				result *= base;
			}
			return result;
		}

		inline double InvLogit2(const double bias){
			return 2/(1+exp(-bias));
		}

		inline bool IsGC(seqan::Dna5String base){
			return base == 'G' || base == 'C';
		}

		// Specify possible options instead of using a template so we do not accidentally call function with seqan::CharString etc., which results in wrong output
		inline bool IsGC(const seqan::Dna5String& sequence, uintSeqLen pos){
			return IsGC( at(sequence, pos) );
		}
		inline bool IsGC(const seqan::DnaString& sequence, uintSeqLen pos){
			return IsGC( at(sequence, pos) );
		}
		inline bool IsGC(const seqan::Dna5StringReverseComplement& sequence, uintSeqLen pos){
			return IsGC( at(sequence, pos) );
		}
		inline bool IsGC(const seqan::DnaStringReverseComplement& sequence, uintSeqLen pos){
			return IsGC( at(sequence, pos) );
		}
		inline bool IsGC(const ConstDna5StringReverseComplement& sequence, uintSeqLen pos){
			return IsGC( at(sequence, pos) );
		}
		inline bool IsGC(const ConstDnaStringReverseComplement& sequence, uintSeqLen pos){
			return IsGC( at(sequence, pos) );
		}

		inline bool IsN(seqan::Dna5 base){
			return 3 < static_cast<uintBaseCall>(base);
		}

		// Specify possible options instead of using a template so we do not accidentally call function with seqan::CharString etc., which results in wrong output
		inline bool IsN(const seqan::Dna5String& sequence, uintSeqLen pos){
			return IsN( at(sequence, pos) );
		}
		inline bool IsN(const seqan::Dna5StringReverseComplement& sequence, uintSeqLen pos){
			return IsN( at(sequence, pos) );
		}
		inline bool IsN(const ConstDna5StringReverseComplement& sequence, uintSeqLen pos){
			return IsN( at(sequence, pos) );
		}

		template<typename T> inline T MeanWithRoundingToFirst(T first, T second){ // Mean between first and second and rounding in the direction of first
			return (first+second+(first>second?static_cast<T>(1):static_cast<T>(0)))/static_cast<T>(2);
		}

		template<typename T, typename U> inline uintPercent Percent(T nom, U den){ // Calculates the percentage: nom*100/den with proper rounding to int
			return Divide(static_cast<T>(nom*100),den);
		}
		template<typename T, typename U> inline uintPercent Percent(const std::atomic<T> &nom, U den){ // Calculates the percentage: nom*100/den with proper rounding to int
			return Divide(static_cast<T>(nom*100),den);
		}

		inline seqan::Dna5String ReverseComplementorDna5(seqan::Dna5String org){
			return ConstDna5StringReverseComplement(org);
		}
		inline seqan::DnaString ReverseComplementorDna(seqan::DnaString org){
			return ConstDnaStringReverseComplement(org);
		}

		template<typename T, typename U> inline uintPercent SafePercent(T nom, U den){
			if(den){
				return Percent(nom,den);
			}
			else{
				return 50;
			}
		}

		template<typename T> void SetDimensions(std::vector<T> &vec, size_t dim1, size_t dim2){
			vec.resize(dim1);
			for(auto i=dim1; i--; ){
				vec.at(i).resize(dim2);
			}
		}

		template<typename T, typename... Args> void SetDimensions(std::vector<T> &vec, size_t dim1, size_t dim2, Args... args){
			vec.resize(dim1);
			for(auto i=dim1; i--; ){
				SetDimensions(vec.at(i), dim2, args...);
			}
		}

		template <typename T> inline int Sign(T val) {
		    return (T(0) < val) - (val < T(0));
		}

		inline uintSeqLen TransformDistanceToStartOfErrorRegion(uintSeqLen start_dist_error_region){
			return (start_dist_error_region+9)/10;
		}

		inline unsigned int TrueRandom(){
			std::random_device rd;
			return rd();
		}
	}
#endif //SWIG
}

#endif // UTILITIES_H

ext: hpp

max_stars_repo_name: schmeing/ReSeq

max_issues_repo_name: schmeing/ReSeq

max_forks_repo_name: schmeing/ReSequenceR


text:
/**********************************************************************************/
/* This file is part of spla project                                              */
/* https://github.com/JetBrains-Research/spla                                     */
/**********************************************************************************/
/* MIT License                                                                    */
/*                                                                                */
/* Copyright (c) 2021 JetBrains-Research                                          */
/*                                                                                */
/* Permission is hereby granted, free of charge, to any person obtaining a copy   */
/* of this software and associated documentation files (the "Software"), to deal  */
/* in the Software without restriction, including without limitation the rights   */
/* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell      */
/* copies of the Software, and to permit persons to whom the Software is          */
/* furnished to do so, subject to the following conditions:                       */
/*                                                                                */
/* The above copyright notice and this permission notice shall be included in all */
/* copies or substantial portions of the Software.                                */
/*                                                                                */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR     */
/* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,       */
/* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE    */
/* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER         */
/* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,  */
/* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE  */
/* SOFTWARE.                                                                      */
/**********************************************************************************/

#include <boost/compute.hpp>
#include <core/SplaLibraryPrivate.hpp>
#include <core/SplaQueueFinisher.hpp>
#include <expression/scalar/SplaScalarDataRead.hpp>
#include <storage/SplaScalarStorage.hpp>
#include <storage/SplaScalarValue.hpp>

bool spla::ScalarDataRead::Select(std::size_t nodeIdx, const spla::Expression &expression) {
    return true;
}

void spla::ScalarDataRead::Process(std::size_t nodeIdx, const spla::Expression &expression, spla::TaskBuilder &builder) {
    auto &nodes = expression.GetNodes();
    auto &node = nodes[nodeIdx];
    auto &library = node->GetLibrary().GetPrivate();

    auto scalar = node->GetArg(0).Cast<Scalar>();
    auto data = node->GetArg(1).Cast<DataScalar>();
    auto desc = node->GetDescriptor();
    auto hostValue = reinterpret_cast<unsigned char *>(data->GetValue());

    assert(scalar);
    assert(data);
    assert(desc);
    assert(hostValue);

    auto deviceValue = scalar->GetStorage()->GetValue();

    if (deviceValue.IsNotNull()) {
        using namespace boost;

        auto &deviceMan = library.GetDeviceManager();
        auto deviceId = deviceMan.FetchDevice(node);

        compute::device device = deviceMan.GetDevice(deviceId);
        compute::context ctx = library.GetContext();
        compute::command_queue queue(ctx, device);
        QueueFinisher finisher(queue);

        compute::copy(deviceValue->GetVal().begin(), deviceValue->GetVal().end(), hostValue, queue);
    }
}

spla::ExpressionNode::Operation spla::ScalarDataRead::GetOperationType() const {
    return ExpressionNode::Operation::ScalarDataRead;
}

ext: cpp

max_stars_repo_name: Glebanister/spla

max_issues_repo_name: Glebanister/spla

max_forks_repo_name: Glebanister/spla


text:
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <cmath>
#include <memory>
#include <vector>
#include <limits>
#include <algorithm>
#include <utility>
#include <iterator>
#include "statespace.hpp"

using namespace boost::numeric::ublas;

ARSSM::ARSSM(vector<double> & params, double error_var,
	     double obs_noise, double bias) : params(params), error_var(error_var),
					      obs_noise(obs_noise), bias(bias) {

  this->is_cssm = false;
  this->max_dimension = params.size();
}

ARSSM::~ARSSM() {
  return;
};

int ARSSM::apply_transition_matrix(const double * x, int k, double * result) {
  double ip = 0;
  for (unsigned i=0; i < this->params.size(); ++i) {
    ip += x[i] * this->params(i);
  }
  result[0] = ip;
  for (unsigned i=1; i < this->max_dimension; ++i) {
    result[i] = x[i-1];
  }
  return this->max_dimension;
}

int ARSSM::apply_transition_matrix(const matrix <double> &X,
				    unsigned int x_row_offset, int k,
				    matrix <double> &result,
				    unsigned int r_row_offset,
				    unsigned int n) {
  for (unsigned j=0; j < n; ++j) {
    double ip = 0;
    for (unsigned i=0; i < this->params.size(); ++i) {
      ip += X(i+x_row_offset,j) * this->params(i);
    }
    result(r_row_offset,j) = ip;
    for (unsigned i=1; i < this->max_dimension; ++i) {
      result(i+r_row_offset,j) = X(i-1+x_row_offset,j);
    }
  }
  return this->max_dimension;
}

void ARSSM::transition_bias(int k, double *result) {
  return;
}

void ARSSM::transition_noise_diag(int k, double *result) {
  result[0] = this->error_var;
  for (unsigned i=1; i < this->max_dimension; ++i) {
    result[i] = 0.0;
  }
}

double ARSSM::apply_observation_matrix(const double *x, int k) {
  return x[0];
}

void ARSSM::apply_observation_matrix(const matrix <double> &X,
				     unsigned int row_offset, int k,
				     double *result, double * result_tmp, unsigned int n) {
  for (unsigned i=0; i < n; ++i) {
    result[i] = X(row_offset, i);
  }
}

double ARSSM::observation_bias(int k) {
  return this->bias;
}

double ARSSM::observation_noise(int k) {
  return this->obs_noise;
}

bool ARSSM::stationary(int k) {
  return true;
}

int ARSSM::prior_mean(double *result) {
  for (unsigned i=0; i < this->max_dimension; ++i) {
    result[i] = 0;
  }
  return this->max_dimension;
}

int ARSSM::prior_vars(double *result) {
  for(unsigned i=0; i < this->max_dimension; ++i) {
    result[i] = this->error_var;
  }
  return this->max_dimension;
}

ext: cc

max_stars_repo_name: davmre/sigvisa

max_issues_repo_name: davmre/sigvisa

max_forks_repo_name: davmre/sigvisa


text:
/* The MIT License (MIT)

   Copyright (c) 2016 Xenofon Foukas

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in all
   copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
   SOFTWARE.
*/

#include <thread>
#include <iostream>

#include <pthread.h>
#include <sched.h>
#include <linux/sched.h>

#include <boost/program_options.hpp>

#include "rt_wrapper.h"

#include "async_xface.h"
#include "flexran.pb.h"
#include "rib_updater.h"
#include "rib.h"
#include "task_manager.h"
#include "stats_manager.h"
#include "remote_scheduler.h"
#include "remote_scheduler_delegation.h"
#include "remote_scheduler_eicic.h"
#include "flexible_scheduler.h"
#include "delegation_manager.h"
#include "requests_manager.h"

// Fort RESTful northbound API
#ifdef REST_NORTHBOUND

#include "call_manager.h"
#include "flexible_sched_calls.h"
#include "stats_manager_calls.h"

#endif

#include <pistache/endpoint.h>

#include <errno.h>
#include <string.h>
#include <stdio.h>

#include "flexran_log.h"

namespace po = boost::program_options;

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

  int cport = 2210;
  int north_port = 9999;

  bool debug = false;

  // Find the root directory
  std::string path = "";

  if(const char* env_p = std::getenv("FLEXRAN_RTC_HOME")) {
    path = env_p;
  } else {
    path = "../";
  }

  try {
    po::options_description desc("Help");
    desc.add_options()
      ("port,p", po::value<int>()->default_value(2210),
       "Port for incoming agent connections")
      ("nport,n", po::value<int>()->default_value(9999),
       "Port for northbound API calls")
      ("help,h", "Prints this help message")
      ("debug,d", "Enables debugging messages to be displayed and logged");

    po::variables_map opts;
    po::store(po::parse_command_line(argc, argv, desc), opts);

    if ( opts.count("help")  ) {
      std::cout << "FlexRAN real-time controller" << std::endl
		<< desc << std::endl;
      return 0;
    }

    if (opts.count("debug")) {
      std::cout << "Debugging enabled" << std::endl;
      debug = true;
    }

    try {
      po::notify(opts);
    } catch (po::error& e) {
      std::cerr << "Error: Unrecognized parameter\n";
      return 1;
    }

    cport = opts["port"].as<int>();
    north_port = opts["nport"].as<int>();

  } catch(std::exception& e) {
    std::cerr << "Error: Unrecognized parameter\n";
    return 2;
  }

  if (!debug) {
    // Initialize the logger with default properties
    flexran_log::PropertyConfigurator::configure(path + "/log_config/basic_log");
  } else {
    // Initialize the logger with debugging properties
    flexran_log::PropertyConfigurator::configure(path + "/log_config/debug_log");
  }

  std::shared_ptr<flexran::app::scheduler::flexible_scheduler> flex_sched_app;

  LOG4CXX_INFO(flexran::core::core_logger, "Listening on port " << cport << " for incoming agent connections");
  flexran::network::async_xface net_xface(cport);

  // Create the rib
  flexran::rib::Rib rib;

  // Create the rib update manager
  flexran::rib::rib_updater r_updater(net_xface, rib);

  // Create the task manager
  flexran::core::task_manager tm(r_updater);

  // Create the requests manager
  flexran::core::requests_manager rm(net_xface);

  // Register any applications that we might want to execute in the controller
  // Stats manager
  std::shared_ptr<flexran::app::component> stats_app(new flexran::app::stats::stats_manager(rib, rm));
  tm.register_app(stats_app);

  // Flexible scheduler
  std::shared_ptr<flexran::app::component> flex_sched(new flexran::app::scheduler::flexible_scheduler(rib, rm));
  tm.register_app(flex_sched);

  /* More examples of developed applications are available in the commented section.
     WARNING: Some of them might still contain bugs or might be from previous versions of the controller. */

  // Remote scheduler
  //std::shared_ptr<flexran::app::component> remote_sched(new flexran::app::scheduler::remote_scheduler(rib, rm));
  //tm.register_app(remote_sched);

  // eICIC remote scheduler
  //std::shared_ptr<flexran::app::component> remote_sched_eicic(new flexran::app::scheduler::remote_scheduler_eicic(rib, rm));
  //tm.register_app(remote_sched_eicic);

  // Remote scheduler with delegation (TEST purposes)
  // std::shared_ptr<flexran::app::component> remote_sched(new flexran::app::scheduler::remote_scheduler_delegation(rib, rm));
  // tm.register_app(remote_sched);

  // Delegation manager (TEST purposes)
  //std::shared_ptr<flexran::app::component> delegation_manager(new flexran::app::management::delegation_manager(rib, rm));
  //tm.register_app(delegation_manager);

  // Start the network thread
  std::thread networkThread(&flexran::network::async_xface::execute_task, &net_xface);

  // Start the task manager thread
  std::thread task_manager_thread(&flexran::core::task_manager::execute_task, &tm);

#ifdef REST_NORTHBOUND

  // Initialize the northbound API

  // Set the port and the IP to listen for REST calls and initialize the call manager
  Pistache::Port port(north_port);
  Pistache::Address addr(Pistache::Ipv4::any(), port);
  flexran::north_api::manager::call_manager north_api(addr);

  // Register API calls for the developed applications
  flexran::north_api::flexible_sched_calls scheduler_calls(std::dynamic_pointer_cast<flexran::app::scheduler::flexible_scheduler>(flex_sched));

  flexran::north_api::stats_manager_calls stats_calls(std::dynamic_pointer_cast<flexran::app::stats::stats_manager>(stats_app));

  north_api.register_calls(scheduler_calls);
  north_api.register_calls(stats_calls);

  // Start the call manager
  north_api.init(1);
  north_api.start();

  north_api.shutdown();

#else
  if (task_manager_thread.joinable())
    task_manager_thread.join();

  if (networkThread.joinable())
    networkThread.join();

#endif
  return 0;
}

max_stars_repo_name: fon/flexran-rtc

max_issues_repo_name: fon/flexran-rtc

max_forks_repo_name: fon/flexran-rtc


text:
#include <boost/numeric/ublas/assignment.hpp>
#include "gridsampling.hpp"

int main()
{
  vectori divs(3);
  divs <<= 3,4,5;

  vecOfvec grid;
  bayesopt::utils::buildGrid(divs,grid);

  for(size_t i=0; i<grid.size(); ++i)
    {
      for(size_t j=0; j<grid[i].size(); ++j)
	{
	  std::cout << grid[i](j) << ", ";
	}
      std::cout << std::endl;
    }
}

max_stars_repo_name: pchrapka/brain-modelling

max_issues_repo_name: pchrapka/brain-modelling

max_forks_repo_name: pchrapka/brain-modelling


text:
// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/core/to.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/filter.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/mod.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/slice.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>
namespace hana = boost::hana;
using namespace hana::literals;

// Slice a contiguous range
constexpr auto xs = hana::make_tuple(0, '1', 2.2, 3_c, hana::type_c<float>);

static_assert(
    hana::slice(xs, hana::tuple_c<std::size_t, 1, 2, 3>) ==
    hana::make_tuple('1', 2.2, 3_c)
, "");

// A more complex example with a non-contiguous range
constexpr auto letters = hana::to_tuple(hana::range_c<char, 'a', 'z'>);
constexpr auto indices = hana::to_tuple(hana::make_range(hana::size_c<0>, hana::length(letters)));

auto even_indices = hana::filter(indices, [](auto n) {
    return n % hana::size_c<2> == hana::size_c<0>;
});

BOOST_HANA_CONSTANT_CHECK(
    hana::slice(letters, even_indices) == hana::tuple_c<char,
        'a', 'c', 'e', 'g', 'i', 'k', 'm', 'o', 'q', 's', 'u', 'w', 'y'
    >
);

int main() { }

max_issues_repo_name: lipper/arangodb

max_forks_repo_name: lipper/arangodb


text:
//  Copyright (c) 2013 Thomas Heller
//  Copyright (c) 2013 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

/// \file polymorphic_factory.hpp

#if !defined(HPX_RUNTIME_UTIL_POLYMORPHIC_FACTORY_HPP)
#define HPX_RUNTIME_UTIL_POLYMORPHIC_FACTORY_HPP

#include <hpx/hpx_fwd.hpp>
#include <hpx/util/static.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/cstdint.hpp>

#include <map>

#include <hpx/config/warnings_prefix.hpp>

namespace hpx { namespace traits
{
    // This trait is used to decide whether a class (or specialization) is
    // required to automatically register to the action factory
    template <typename T, typename Enable = void>
    struct needs_automatic_registration
      : boost::mpl::true_
    {};
}}

namespace hpx { namespace actions { namespace detail
{
    ///////////////////////////////////////////////////////////////////////////
    template <typename Action>
    struct action_registration;

    template <typename Continuation>
    struct continuation_registration;
}}}

namespace hpx { namespace util
{
    namespace detail
    {
        template <typename VTable, typename T>
        struct function_registration;
    }

    ///////////////////////////////////////////////////////////////////////////
    template <typename Base>
    class HPX_EXPORT polymorphic_factory
    {
    public:
        typedef boost::shared_ptr<Base>(*ctor_type)();
        typedef std::multimap<
            boost::uint32_t, std::pair<std::string, ctor_type>
        > ctor_map;

        static boost::shared_ptr<Base> create(std::string const & name);

    protected:
        typename ctor_map::const_iterator locate(boost::uint32_t hash,
            std::string const& name) const;

    private:
        void add_factory_function(std::string const & name, ctor_type ctor);
        static polymorphic_factory& get_instance();

        ctor_map ctor_map_;

        template <typename Action>
        friend struct actions::detail::action_registration;

        template <typename Continuation>
        friend struct actions::detail::continuation_registration;

        template <typename VTable, typename T>
        friend struct util::detail::function_registration;
    };
}}

#include <hpx/config/warnings_suffix.hpp>

#endif


text:
#pragma once

#include <chrono>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <sstream>
#include <types.hpp>

namespace cut {

	class clock {

	private:
		std::chrono::system_clock::time_point begin;
		std::chrono::system_clock::time_point end;
		double result_val;

	public:
		clock();
		void start();
		double stop();
		double result();

	};

	class json_parser {

	private:
		boost::property_tree::ptree p_tree;

	public:
		json_parser(std::string file_name);

		void reset(std::string file_name);

		template <typename _return_type>
		_return_type get_value(std::string key)
		{
			if (boost::optional<_return_type> value = p_tree.get_optional<_return_type>(key)) {
				return value.get();
			}

			std::cerr << "Error. FAILD TO PARSE JSON FILE. KEY NOT FOUND -> " << key << "." << std::endl;
			exit(-1);
		}

		bool is_exist(std::string key);

		std::vector<std::string> json_parser::get_children(std::string key, std::string child_key)
		{
			std::vector<std::string> result;

			BOOST_FOREACH(const boost::property_tree::ptree::value_type & child, p_tree.get_child(key)) {
				const boost::property_tree::ptree & info = child.second;

				if (boost::optional<std::string> object = info.get_optional<std::string>(child_key))
				{
					result.push_back(object.get());
				}
				else {
					std::cerr << "FAILD TO PARSE. IN FUNCTION json_parser::get_children" << std::endl;
					exit(-1);
				}
			}

			return result;
		}

	};

	class Jouseki {

	private:

		template <typename _return_type>
		void load_json_elem(cut::json_parser & parser, std::string parent_key, std::string child_key, std::vector<_return_type>  *dish)
		{
			std::vector<std::string> iti_vector;
			iti_vector = parser.get_children(parent_key, child_key);
			for (std::string elem : iti_vector) {
				dish->push_back((_return_type)std::stoi(elem));
			}
		}

	public:
		Jouseki(std::string file_name);
		std::vector<Te> jouseki_list;

	};

	BANMEN *load_position(std::stringstream & ss);
	BANMEN *create_start_position();

}

max_stars_repo_name: atsisy/coyuri-jc-backend

max_issues_repo_name: atsisy/coyuri-jc-backend

max_forks_repo_name: atsisy/coyuri-jc-backend


text:
/*----------------------THE GEORGE WASHINGTON UNIVERSITY-----------------------
author: James R. Taylor                                             jrt@gwu.edu

dynamics.cpp
Moby plugin
-----------------------------------------------------------------------------*/
///*
#include <stdio.h>

#include <errno.h>
#include <sys/time.h>
#include <dlfcn.h>
#include <iostream>
#include <cmath>
#include <fstream>
#include <boost/foreach.hpp>
#include <Moby/XMLReader.h>
#include <Ravelin/AAngled.h>

#ifdef USE_OSG
#include <osgViewer/Viewer>
#include <osgViewer/ViewerEventHandlers>
#include <osg/Geode>
#include <osgDB/ReadFile>
#include <osgDB/WriteFile>
#include <osgGA/TrackballManipulator>
#include <osgGA/StateSetManipulator>
#endif

#include <Moby/Log.h>
#include <Moby/Simulator.h>
#include <Moby/RigidBody.h>
#include <Moby/EventDrivenSimulator.h>
#include <Moby/RCArticulatedBody.h>
//*/
#include <tcs/dynamics_data.h>
#include <tcs/time.h>
//#include "moby_xml.h"
/*
#include <tas/tas.h>
#include <tas/actuator.h>
#include <tas/log.h>
#include <tas/ipc.h>
*/

#include "../moby_space.h"
#include "../ship.h"

typedef double Real;

//-----------------------------------------------------------------------------

using namespace Moby;
using boost::shared_ptr;
using boost::dynamic_pointer_cast;

//-----------------------------------------------------------------------------

//log_c dyn_error_log;
char spstr[ 512 ];

//-----------------------------------------------------------------------------
// MOBY driver
//-----------------------------------------------------------------------------

/// Handle for dynamic library loading
void* HANDLE = NULL;

/// Horizontal and vertical resolutions for offscreen-rendering
const unsigned HORZ_RES = 1024;
const unsigned VERT_RES = 768;

/// Beginning iteration for logging
unsigned LOG_START = 0;

/// Ending iteration for logging
unsigned LOG_STOP = std::numeric_limits<unsigned>::max();

/// The logging reporting level
unsigned LOG_REPORTING_LEVEL = 0;

/// Used for timing
clock_t start_time;

/// The default simulation step size
const Real DEFAULT_STEP_SIZE = .001;

/// The simulation step size
Real STEP_SIZE = DEFAULT_STEP_SIZE;

/// The time of the first simulation step
Real FIRST_STEP_TIME = -1;

/// The time of the last simulation step
Real LAST_STEP_TIME = 0;

/// The current simulation iteration
unsigned ITER = 0;

/// Interval for offscreen renders (0=offscreen renders disabled)
unsigned IMAGE_IVAL = 0;

/// Interval for 3D outputs (0=3D outputs disabled)
unsigned THREED_IVAL = 0;

/// Determines whether to do onscreen rendering (false by default)
bool ONSCREEN_RENDER = false;

/// Determines whether to output timings
bool OUTPUT_TIMINGS = false;

/// Extension/format for 3D outputs (default=Wavefront obj)
char THREED_EXT[5] = "obj";

/// Determines whether to update graphics (false by default, but certain
/// options will set to true)
bool UPDATE_GRAPHICS = false;

/// If true, outputs (image, vrml) tied to simulation time rather than actual iteration count
bool OUTPUT_TO_TIME = false;

/// The maximum number of iterations (default infinity)
unsigned MAX_ITER = std::numeric_limits<unsigned>::max();

/// The maximum time of the simulation (default infinity)
Real MAX_TIME = std::numeric_limits<Real>::max();

/// The total (CPU) clock time used by the simulation
Real TOTAL_TIME = 0.0;

/// Last 3D output iteration and time output
unsigned LAST_3D_WRITTEN = -1;
Real LAST_3D_WRITTEN_T = -std::numeric_limits<Real>::max()/2.0;

/// Last image iteration output
unsigned LAST_IMG_WRITTEN = -1;
Real LAST_IMG_WRITTEN_T = -std::numeric_limits<Real>::max()/2.0;

/// Outputs to stdout
bool OUTPUT_FRAME_RATE = false;
bool OUTPUT_ITER_NUM = false;
bool OUTPUT_SIM_RATE = false;

/// Render Contact Points
bool RENDER_CONTACT_POINTS = false;

/// The map of objects read from the simulation XML file
std::map<std::string, BasePtr> READ_MAP;

#ifdef USE_OSG
/// The OpenInventor group node for Moby
osg::Group* MOBY_GROUP;

/// The OpenInventor root group node for this application
osg::Group* MAIN_GROUP;

/// Pointer to the viewer
osgViewer::Viewer* viewer_pointer;
#endif

/**
  // Note: In this architecture, Moby doesn't have any direct relationship to
  // controllers, so the ability to load control plugins is removed

/// Pointer to the controller's initializer, called once (if any)
typedef void (*init_t)(void*, const std::map<std::string, BasePtr>&, Real);
std::list<init_t> INIT;
*/
/*
extern bool ONSCREEN_RENDER;
extern std::map<std::string, BasePtr> READ_MAP;
#ifdef USE_OSG
extern osgViewer::Viewer* viewer_pointer;
#endif
*/

//-----------------------------------------------------------------------------
// Maybe should be inside extern "C" scope
static moby_space_p space;
static ship_p predator;
static ship_p prey;

std::vector<double> prey_state;
std::vector<double> pred_state;
std::vector<double> prey_control;
std::vector<double> pred_control;

//-----------------------------------------------------------------------------

/// Checks whether was compiled with OpenSceneGraph support
bool check_osg() {
 #ifdef USE_OSG
  return true;
 #else
  return false;
 #endif
}

//-----------------------------------------------------------------------------

/// TODO : REFACTOR using TAS Standard
/// Gets the current time (as a floating-point number)
Real get_current_time() {
  const Real MICROSEC = 1.0/1000000;
  timeval t;
  gettimeofday(&t, NULL);
  return (Real) t.tv_sec + (Real) t.tv_usec * MICROSEC;
}

//-----------------------------------------------------------------------------

/// runs the simulator and updates all transforms
void step(void* arg) {
  // get the simulator pointer
  boost::shared_ptr<Simulator> sim = *(boost::shared_ptr<Simulator>*) arg;

  // get the simulator as event driven simulation
  boost::shared_ptr<EventDrivenSimulator> edsim = boost::dynamic_pointer_cast<EventDrivenSimulator>( sim );

  // see whether to activate logging
  if (ITER >= LOG_START && ITER <= LOG_STOP)
    Log<OutputToFile>::reporting_level = LOG_REPORTING_LEVEL;
  else
    Log<OutputToFile>::reporting_level = 0;

    // output the iteration #
  if (OUTPUT_ITER_NUM)
    std::cout << "iteration: " << ITER << "  simulation time: " << sim->current_time << std::endl;
  if (Log<OutputToFile>::reporting_level > 0)
    FILE_LOG(Log<OutputToFile>::reporting_level) << "iteration: " << ITER << "  simulation time: " << sim->current_time << std::endl;

  // only update the graphics if it is necessary
  if (UPDATE_GRAPHICS)
    sim->update_visualization();

  // output the image, if desired
 #ifdef USE_OSG
  if (IMAGE_IVAL > 0) {
    // determine at what iteration nearest frame would be output
    if ((OUTPUT_TO_TIME && sim->current_time - LAST_IMG_WRITTEN_T > STEP_SIZE * IMAGE_IVAL) || (!OUTPUT_TO_TIME && ITER % IMAGE_IVAL == 0)) {
      char buffer[128];
      sprintf(buffer, "driver.out.%08u.png", ++LAST_IMG_WRITTEN);
      // TODO: call offscreen renderer
      LAST_IMG_WRITTEN_T = sim->current_time;
    }
  }

  // output the 3D file, if desired
  if (THREED_IVAL > 0) {
    // determine at what iteration nearest frame would be output
    if ((OUTPUT_TO_TIME && sim->current_time - LAST_3D_WRITTEN_T > STEP_SIZE * THREED_IVAL) || (!OUTPUT_TO_TIME && ITER % THREED_IVAL == 0)) {
      // write the file (fails silently)
      char buffer[128];
      sprintf(buffer, "driver.out-%08u-%f.%s", ++LAST_3D_WRITTEN, sim->current_time, THREED_EXT);
      osgDB::writeNodeFile(*MAIN_GROUP, std::string(buffer));
      LAST_3D_WRITTEN_T = sim->current_time;
    }
  }
  #endif

  // step the simulator and update visualization
  clock_t pre_sim_t = clock();
  sim->step(STEP_SIZE);
  clock_t post_sim_t = clock();
  Real total_t = (post_sim_t - pre_sim_t) / (Real) CLOCKS_PER_SEC;
  TOTAL_TIME += total_t;

  // output the iteration / stepping rate
  if (OUTPUT_SIM_RATE)
    std::cout << "time to compute last iteration: " << total_t << " (" << TOTAL_TIME / ITER << "s/iter, " << TOTAL_TIME / sim->current_time << "s/step)" << std::endl;
/*
  // see whether to output the timings
  if (OUTPUT_TIMINGS) {
    if (!edsim)
      std::cout << ITER << " " << sim->dynamics_utime << " " << sim->dynamics_stime << std::endl;
    else
      std::cout << ITER << " " << edsim->dynamics_utime << " " << edsim->dynamics_stime << " " << edsim->coldet_utime << " " << edsim->coldet_stime << " " << edsim->event_utime << " " << edsim->event_stime << std::endl;
  }
*/
  // update the iteration #
  ITER++;

  // output the frame rate, if desired
  if (OUTPUT_FRAME_RATE) {
    Real tm = get_current_time();
    std::cout << "instantaneous frame rate: " << (1.0/(tm - LAST_STEP_TIME)) << "fps  avg. frame rate: " << (ITER / (tm - FIRST_STEP_TIME)) << "fps" << std::endl;
    LAST_STEP_TIME = tm;
  }

  // check that maximum number of iterations or maximum time not exceeded
  if (ITER >= MAX_ITER || sim->current_time > MAX_TIME) {
    clock_t end_time = clock();
    Real elapsed = (end_time - start_time) / (Real) CLOCKS_PER_SEC;
    std::cout << elapsed << " seconds elapsed" << std::endl;
    exit(0);
  }

  // if render contact points enabled, notify the Simulator
  if( RENDER_CONTACT_POINTS && edsim)
    edsim->render_contact_points = true;
}

//-----------------------------------------------------------------------------

/// Adds lights to the scene when no scene background file specified
void add_lights() {
 #ifdef USE_OSG
  // add lights
 #endif
}

//-----------------------------------------------------------------------------
///*
/// Gets the XML sub-tree rooted at the specified tag
shared_ptr<const XMLTree> find_subtree(shared_ptr<const XMLTree> root, const std::string& name) {

  // if we found the tree, return it
  if (strcasecmp(root->name.c_str(), name.c_str()) == 0)
    return root;

  // otherwise, look for it recursively
  const std::list<XMLTreePtr>& children = root->children;
  for (std::list<XMLTreePtr>::const_iterator i = children.begin(); i != children.end(); i++) {
    shared_ptr<const XMLTree> node = find_subtree(*i, name);
    if (node)
      return node;
  }

  // return NULL if we are here
  return shared_ptr<const XMLTree>();
}
//*/
//-----------------------------------------------------------------------------
///*
// finds and processes given XML tags
void process_tag(const std::string& tag, shared_ptr<const XMLTree> root, void (*fn)(shared_ptr<const XMLTree>)) {
  // if this node is of the given type, process it
  if (strcasecmp(root->name.c_str(), tag.c_str()) == 0) {
    fn(root);
  } else {
    const std::list<XMLTreePtr>& child_nodes = root->children;
    for (std::list<XMLTreePtr>::const_iterator i = child_nodes.begin(); i != child_nodes.end(); i++)
      process_tag(tag, *i, fn);
  }
}
//*/
//-----------------------------------------------------------------------------
///*
/// processes the 'camera' tag
void process_camera_tag(boost::shared_ptr<const XMLTree> node) {
  if (!ONSCREEN_RENDER)
    return;

  // read all attributes
  XMLAttrib* target_attr = node->get_attrib("target");
  XMLAttrib* position_attr = node->get_attrib("position");
  XMLAttrib* up_attr = node->get_attrib("up");
  if (!target_attr || !position_attr || !up_attr)
    return;

  // get the actual values
  Ravelin::Vector3d target, position, up;
  target_attr->get_vector_value(target);
  position_attr->get_vector_value(position);
  up_attr->get_vector_value(up);

  // setup osg vectors
 #ifdef USE_OSG
  osg::Vec3d position_osg(position[0], position[1], position[2]);
  osg::Vec3d target_osg(target[0], target[1], target[2]);
  osg::Vec3d up_osg(up[0], up[1], up[2]);

  // set the camera view
  if (viewer_pointer && viewer_pointer->getCameraManipulator()) {
    osg::Camera* camera = viewer_pointer->getCamera();
    camera->setViewMatrixAsLookAt(position_osg, target_osg, up_osg);

    // setup the manipulator using the camera, if necessary
    viewer_pointer->getCameraManipulator()->setHomePosition(position_osg, target_osg, up_osg);
  }
 #endif
}
//*/
//-----------------------------------------------------------------------------
///*
/// processes the 'window' tag
void process_window_tag(boost::shared_ptr<const XMLTree> node) {
  // don't process if not onscreen rendering
  if (!ONSCREEN_RENDER)
    return;

  // read window location
  XMLAttrib* loc_attr = node->get_attrib("location");

  // read window size
  XMLAttrib* size_attr = node->get_attrib("size");

  // get the actual values
  Ravelin::Vector2d loc(0,0), size(640,480);
  if (loc_attr)
    loc_attr->get_vector_value(loc);
  if (size_attr)
    size_attr->get_vector_value(size);

 #ifdef USE_OSG
  // setup the window
  viewer_pointer->setUpViewInWindow(loc[0], loc[1], size[0], size[1]);
 #endif
}
//*/
//-----------------------------------------------------------------------------
///*
/// processes all 'driver' options in the XML file
void process_xml_options(const std::string& xml_fname) {
  // *************************************************************
  // going to remove any path from the argument and change to that
  // path; this is done so that all files referenced from the
  // local path of the XML file are found
  // *************************************************************

  // set the filename to use as the argument, by default
  std::string filename = xml_fname;

  // get the current pathname
  size_t BUFSIZE = 128;
  boost::shared_array<char> cwd;
  while (true) {
    cwd = boost::shared_array<char>((new char[BUFSIZE]));
    if (getcwd(cwd.get(), BUFSIZE) == cwd.get())
      break;
    if (errno != ERANGE) {
      std::cerr << "process_xml_options() - unable to allocate sufficient memory!" << std::endl;
      return;
    }
    BUFSIZE *= 2;
  }

  // separate the path from the filename
  size_t last_path_sep = xml_fname.find_last_of('/');
  if (last_path_sep != std::string::npos) {
    // get the new working path
    std::string pathname = xml_fname.substr(0,last_path_sep+1);

    // change to the new working path
    if( chdir(pathname.c_str()) == -1 ) {
      // Error
     }

    // get the new filename
    filename = xml_fname.substr(last_path_sep+1,std::string::npos);
  }

  // read the XML Tree
  //XMLTreePtr driver_tree = XMLTree::read_from_xml(filename);
  shared_ptr<const XMLTree> driver_tree = XMLTree::read_from_xml(filename);
  if (!driver_tree) {
    std::cerr << "process_xml_options() - unable to open file " << xml_fname;
    std::cerr << " for reading" << std::endl;
    chdir(cwd.get());
    return;
  }

  // find the driver tree
  driver_tree = find_subtree(driver_tree, "driver");

  // make sure that the driver node was found
  if (!driver_tree) {
    if( chdir(cwd.get()) == -1 ) {
            // Error
    }
    return;
  }

  // process tags
  process_tag("window", driver_tree, process_window_tag);
  process_tag("camera", driver_tree, process_camera_tag);

  // change back to current directory
  if( chdir(cwd.get()) == -1 ) {
    // Error
  }
}
//*/
//-----------------------------------------------------------------------------
// MOBY as a plugin.
// c_dynamics_plugin
//-----------------------------------------------------------------------------

// Plugins have to extern "C"
extern "C" {

//-----------------------------------------------------------------------------

boost::shared_ptr<Simulator> sim;
osgViewer::Viewer viewer;
const Real DYNAMICS_FREQ = 0.001;
//RCArticulatedBodyPtr pendulum;

//-----------------------------------------------------------------------------
// Dynamics plugin entry point.
bool init( int argc, char** argv ) {
/*
  //+++++++++++++++++++++++++++++++++++++++
  dyn_error_log = log_c( FD_ERROR_LOG );
  if( dyn_error_log.open( ) != LOG_ERROR_NONE ) {
    // Note: this is not really necessary
    printf( "(dynamics.cpp) ERROR: init(argc,argv) failed calling log_c.open() on FD_ERROR_LOG\n" );
  }
  //+++++++++++++++++++++++++++++++++++++++
*/
  const unsigned ONECHAR_ARG = 3, TWOCHAR_ARG = 4;

 #ifdef USE_OSG
  viewer.setThreadingModel(osgViewer::Viewer::SingleThreaded);
  viewer_pointer = &viewer;
 #endif

  // setup some default options
  std::string scene_path;
  THREED_IVAL = 0;
  IMAGE_IVAL = 0;

  // check that syntax is ok
  if (argc < 2) {
    std::cerr << "syntax: driver [OPTIONS] <xml file>" << std::endl;
    std::cerr << "        (see README for OPTIONS)" << std::endl;
    return false;
  }

  // get all options
  for (int i=1; i< argc-1; i++) {
    // get the option
    std::string option(argv[i]);

    // process options
    if (option == "-r") {
      ONSCREEN_RENDER = true;
      UPDATE_GRAPHICS = true;
      check_osg();
    } else if (option.find("-of") != std::string::npos) {
      OUTPUT_FRAME_RATE = true;
    } else if (option.find("-ot") != std::string::npos) {
      OUTPUT_TIMINGS = true;
    } else if (option.find("-oi") != std::string::npos) {
      OUTPUT_ITER_NUM = true;
    } else if (option.find("-or") != std::string::npos) {
      OUTPUT_SIM_RATE = true;
    } else if (option.find("-v=") != std::string::npos) {
      UPDATE_GRAPHICS = true;
      check_osg();
      THREED_IVAL = std::atoi(&argv[i][ONECHAR_ARG]);
      //assert(THREED_IVAL >= 0);     // Note: remarked because always true
    } else if (option.find("-i=") != std::string::npos) {
      check_osg();
      UPDATE_GRAPHICS = true;
      IMAGE_IVAL = std::atoi(&argv[i][ONECHAR_ARG]);
      //assert(IMAGE_IVAL >= 0);      // Note: remarked because always true
    } else if (option.find("-t") != std::string::npos) {
      OUTPUT_TO_TIME = true;
    } else if (option.find("-s=") != std::string::npos) {
      STEP_SIZE = std::atof(&argv[i][ONECHAR_ARG]);
      assert(STEP_SIZE >= 0.0 && STEP_SIZE < 1);
    } else if (option.find("-lf=") != std::string::npos) {
      std::string fname(&argv[i][TWOCHAR_ARG]);
      OutputToFile::stream.open(fname.c_str());
    } else if (option.find("-l=") != std::string::npos) {
      LOG_REPORTING_LEVEL = std::atoi(&argv[i][ONECHAR_ARG]);
      Log<OutputToFile>::reporting_level = LOG_REPORTING_LEVEL;
    } else if (option.find("-lt=") != std::string::npos) {
      LOG_START = std::atoi(&argv[i][TWOCHAR_ARG]);
    } else if (option.find("-lp=") != std::string::npos) {
      LOG_STOP = std::atoi(&argv[i][TWOCHAR_ARG]);
    } else if (option.find("-mi=") != std::string::npos) {
      MAX_ITER = std::atoi(&argv[i][TWOCHAR_ARG]);
      assert(MAX_ITER > 0);
    } else if (option.find("-mt=") != std::string::npos) {
      MAX_TIME = std::atof(&argv[i][TWOCHAR_ARG]);
      assert(MAX_TIME > 0);
    } else if (option.find("-x=") != std::string::npos) {
      check_osg();
      scene_path = std::string(&argv[i][ONECHAR_ARG]);
    } else if (option.find("-p=") != std::string::npos) {
      /// read_plugin(&argv[i][ONECHAR_ARG]);
    } else if (option.find("-y=") != std::string::npos) {
      strcpy(THREED_EXT, &argv[i][ONECHAR_ARG]);
    } else if (option.find("-vcp") != std::string::npos) {
      RENDER_CONTACT_POINTS = true;
    }
  }

  // setup the simulation
  READ_MAP = XMLReader::read(std::string(argv[argc-1]));

  // setup the offscreen renderer if necessary
 #ifdef USE_OSG
  if (IMAGE_IVAL > 0) {
    // TODO: setup offscreen renderer here
  }
 #endif

  // get the (only) simulation object

  for (std::map<std::string, BasePtr>::const_iterator i = READ_MAP.begin(); i != READ_MAP.end(); i++) {
    sim = boost::dynamic_pointer_cast<Simulator>(i->second);
    if (sim)
      break;
  }

  // make sure that a simulator was found
  if (!sim) {
    std::cerr << "driver: no simulator found in " << argv[argc-1] << std::endl;
    //return -1;
    return false;
  }

  // setup osg window if desired
 #ifdef USE_OSG
  if (ONSCREEN_RENDER) {
    // setup any necessary handlers here
    viewer.setCameraManipulator(new osgGA::TrackballManipulator());
    viewer.addEventHandler(new osgGA::StateSetManipulator(viewer.getCamera()->getOrCreateStateSet()));
    viewer.addEventHandler(new osgViewer::WindowSizeHandler);
    viewer.addEventHandler(new osgViewer::StatsHandler);
  }

  // init the main group
  MAIN_GROUP = new osg::Group;
 #endif

  // look for a scene description file
 #ifdef USE_OSG
  if (scene_path != "") {
    std::ifstream in(scene_path.c_str());
    if (in.fail()) {
      std::cerr << "driver: unable to find scene description from " << scene_path << std::endl;
      add_lights();
    } else {
      in.close();
      osg::Node* node = osgDB::readNodeFile(scene_path);
      if (!node) {
        std::cerr << "driver: unable to open scene description file!" << std::endl;
        add_lights();
    } else
      MAIN_GROUP->addChild(node);
    }
  } else {
    add_lights();
  }
 #endif

  // process XML options
  process_xml_options(std::string(argv[argc-1]));

  // get the simulator visualization
 #ifdef USE_OSG
  MAIN_GROUP->addChild(sim->get_persistent_vdata());
  MAIN_GROUP->addChild(sim->get_transient_vdata());
 #endif

  // setup the timers
  FIRST_STEP_TIME = get_current_time();
  LAST_STEP_TIME = FIRST_STEP_TIME;

  // begin timing
  start_time = clock();

  // prepare to render
 #ifdef USE_OSG
  if (ONSCREEN_RENDER) {
    viewer.setSceneData(MAIN_GROUP);
    viewer.realize();
  }
 #endif

  // custom implementation follows

  space = moby_space_p( new moby_space_c() );
  if( !space->read( READ_MAP ) ) printf( "ERROR: failed to read dynamics\n" );
  prey = space->prey();
  predator = space->predator();

  prey_state.resize( 13 );
  pred_state.resize( 13 );
  prey_control.resize( 6 );
  pred_control.resize( 6 );

  for( unsigned i = 0; i < 13; i++ ) {
    prey_state[i] = 0;
    pred_state[i] = 0;
  }
  prey_state[6] = 1;  // set quaternion w coordinate
  pred_state[6] = 1;  // set quaternion w coordinate

  for( unsigned i = 0; i < 6; i++ ) {
    prey_control[i] = 0;
    pred_control[i] = 0;
  }

  printf( "(dynamics::initialized)\n" );

  return true;
}

//-----------------------------------------------------------------------------

void shutdown( void ) {

}

//-----------------------------------------------------------------------------
void get_state( dynamics_data_t* data ) {
  timestamp_t ts;

  prey->read( prey_state );
  predator->read( pred_state );

  for( unsigned i = 0; i < 7; i++ ) {
    data->prey_state.q[i] = prey_state[i];
    data->pred_state.q[i] = pred_state[i];
  }
  for( unsigned i = 0; i < 6; i++ ) {
    data->prey_state.dq[i] = prey_state[i+7];
    data->pred_state.dq[i] = pred_state[i+7];
  }
  data->prey_state.ts = ts;
  data->pred_state.ts = ts;

}
//-----------------------------------------------------------------------------

// Step the dynamics forward
void step( dynamics_data_t* data ) {
  timestamp_t ts;

 #ifdef USE_OSG
  if( ONSCREEN_RENDER ) {
    if( viewer.done( ) ) return;
      viewer.frame( );
  }
 #endif

  // reset accumulators
  prey->reset();
  predator->reset();

  // map the controls in
  for( unsigned i = 0; i < 6; i++ ) {
    prey_control[i] = data->prey_control.u[i];
    pred_control[i] = data->pred_control.u[i];
  }
  prey->write( prey_control );
  predator->write( pred_control );

  // step dynamics
  step( (void*) &sim );

  // update time data
  data->t = sim->current_time;
  data->dt = STEP_SIZE;
  ts = generate_timestamp();

  // map the state out
  prey->read( prey_state );
  predator->read( pred_state );

  for( unsigned i = 0; i < 7; i++ ) {
    data->prey_state.q[i] = prey_state[i];
    data->pred_state.q[i] = pred_state[i];
  }
  for( unsigned i = 0; i < 6; i++ ) {
    data->prey_state.dq[i] = prey_state[i+7];
    data->pred_state.dq[i] = pred_state[i+7];
  }
  data->prey_state.ts = ts;
  data->pred_state.ts = ts;

  prey->read_ke( data->prey_ke );
  predator->read_ke( data->pred_ke );
}

//-----------------------------------------------------------------------------
Real get_step_size( void ) {
  return STEP_SIZE;
}

//-----------------------------------------------------------------------------

} // extern "C"

//-----------------------------------------------------------------------------

max_stars_repo_name: PositronicsLab/tcs

max_issues_repo_name: PositronicsLab/tcs

max_forks_repo_name: PositronicsLab/tcs


text:
/*
 * Copyright (C) 2019-2020 LEIDOS.
 *
 * 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 <ros/ros.h>
#include <string>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include "route_following_plugin.h"
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_extension/traffic_rules/CarmaUSTrafficRules.h>

namespace route_following_plugin
{
    RouteFollowingPlugin::RouteFollowingPlugin() : mvr_duration_(16.0), current_speed_(0.0) { }
    void RouteFollowingPlugin::initialize()
    {

        nh_.reset(new ros::CARMANodeHandle());
        pnh_.reset(new ros::CARMANodeHandle("~"));
        pnh2_.reset(new ros::CARMANodeHandle("/"));

        plan_maneuver_srv_ = nh_->advertiseService("plugins/RouteFollowing/plan_maneuvers", &RouteFollowingPlugin::plan_maneuver_cb, this);

        plugin_discovery_pub_ = nh_->advertise<cav_msgs::Plugin>("plugin_discovery", 1);
        plugin_discovery_msg_.name = "RouteFollowing";
        plugin_discovery_msg_.versionId = "v1.0";
        plugin_discovery_msg_.available = true;
        plugin_discovery_msg_.activated = true;
        plugin_discovery_msg_.type = cav_msgs::Plugin::STRATEGIC;
        plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers";
        pose_sub_ = nh_->subscribe("current_pose", 1, &RouteFollowingPlugin::pose_cb, this);
        twist_sub_ = nh_->subscribe("current_velocity", 1, &RouteFollowingPlugin::twist_cd, this);

        pnh_->param<double>("minimal_maneuver_duration", mvr_duration_, 16.0);
        pnh2_->param<double>("config_speed_limit",config_limit);
        wml_.reset(new carma_wm::WMListener());
        // set world model point form wm listener
        wm_ = wml_->getWorldModel();
        ros::CARMANodeHandle::setSpinCallback([this]() -> bool
        {
           plugin_discovery_pub_.publish(plugin_discovery_msg_);
            return true;
        });
    }
    void RouteFollowingPlugin::run()
    {
        initialize();
        ros::CARMANodeHandle::spin();
    }
    bool RouteFollowingPlugin::plan_maneuver_cb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp)
    {
        lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y);
        auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 10);
        if(current_lanelets.size() == 0)
        {
            ROS_WARN_STREAM("Cannot find any lanelet in map!");
            return true;
        }

        auto shortest_path = wm_->getRoute()->shortestPath();
        lanelet::ConstLanelet current_lanelet = current_lanelets[0].second;
        int last_lanelet_index = -1;
        for (auto llt : current_lanelets)
        {
            if (boost::geometry::within(current_loc, llt.second.polygon2d()))
            {
                int potential_index = findLaneletIndexFromPath(llt.second.id(), shortest_path);
                if (potential_index != -1)
                {
                    last_lanelet_index = potential_index;
                    break;
                }
            }
        }
        if(last_lanelet_index == -1)
        {
            ROS_ERROR_STREAM("Current position is not on the shortest path! Returning an empty maneuver");
            return true;
        }
        double current_progress = wm_->routeTrackPos(current_loc).downtrack;
        double speed_progress = current_speed_;

        double target_speed=findSpeedLimit(current_lanelet);   //get Speed Limit

        double total_maneuver_length = current_progress + mvr_duration_ * target_speed;

        while(current_progress < total_maneuver_length && last_lanelet_index < shortest_path.size())
        {
            ROS_DEBUG_STREAM("Lanlet: " << shortest_path[last_lanelet_index].id());
            auto p = shortest_path[last_lanelet_index].centerline2d().back();
            double end_dist = wm_->routeTrackPos(shortest_path[last_lanelet_index].centerline2d().back()).downtrack;
            double dist_diff = end_dist - current_progress;
            resp.new_plan.maneuvers.push_back(
                composeManeuverMessage(current_progress, end_dist,
                                       speed_progress, target_speed,
                                       shortest_path[last_lanelet_index].id(), ros::Time::now()));
            current_progress += dist_diff;
            speed_progress = target_speed;
            //get speed limit
            current_lanelet=shortest_path[last_lanelet_index];  //update current lanelet
            target_speed=findSpeedLimit(current_lanelet);

            if(current_progress >= total_maneuver_length || last_lanelet_index == shortest_path.size() - 1)
            {
                break;
            }
            auto following_lanelets = wm_->getRoute()->followingRelations(shortest_path[last_lanelet_index]);
            if(following_lanelets.size() == 0)
            {
                ROS_WARN_STREAM("Cannot find the following lanelet.");
                return true;
            }
            if(identifyLaneChange(following_lanelets, shortest_path[last_lanelet_index + 1].id()))
            {
                ++last_lanelet_index;
            }
            else
            {
                ROS_WARN_STREAM("Cannot find the next lanelet in the current lanelet's successor list!");
                return true;
            }
        }
        if(resp.new_plan.maneuvers.size() == 0)
        {
            ROS_WARN_STREAM("Cannot plan maneuver because no route is found");
        }
        return true;
    }
    void RouteFollowingPlugin::pose_cb(const geometry_msgs::PoseStampedConstPtr& msg)
    {
        pose_msg_ = geometry_msgs::PoseStamped(*msg.get());
    }
    void RouteFollowingPlugin::twist_cd(const geometry_msgs::TwistStampedConstPtr& msg)
    {
        current_speed_ = msg->twist.linear.x;
    }
    int RouteFollowingPlugin::findLaneletIndexFromPath(int target_id, lanelet::routing::LaneletPath& path)
    {
        for(size_t i = 0; i < path.size(); ++i)
        {
            if(path[i].id() == target_id)
            {
                return i;
            }
        }
        return -1;
    }
    cav_msgs::Maneuver RouteFollowingPlugin::composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time current_time)
    {
        cav_msgs::Maneuver maneuver_msg;
        maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING;
        maneuver_msg.lane_following_maneuver.parameters.neogition_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION;
        maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN;
        maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "InLaneCruisingPlugin";
        maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "RouteFollowingPlugin";
        maneuver_msg.lane_following_maneuver.start_dist = current_dist;
        maneuver_msg.lane_following_maneuver.start_speed = current_speed;
        maneuver_msg.lane_following_maneuver.start_time = current_time;
        maneuver_msg.lane_following_maneuver.end_dist = end_dist;
        maneuver_msg.lane_following_maneuver.end_speed = target_speed;
        // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet
        double cur_plus_target = current_speed + target_speed;
        if (cur_plus_target < 0.00001) {
            maneuver_msg.lane_following_maneuver.end_time = current_time + ros::Duration(mvr_duration_);
        } else {
            maneuver_msg.lane_following_maneuver.end_time = current_time + ros::Duration((end_dist - current_dist) / (0.5 * cur_plus_target));
        }
        maneuver_msg.lane_following_maneuver.lane_id = std::to_string(lane_id);
        return maneuver_msg;
    }
    bool RouteFollowingPlugin::identifyLaneChange(lanelet::routing::LaneletRelations relations, int target_id)
    {
        for(auto& relation : relations)
        {
            if(relation.lanelet.id() == target_id && relation.relationType == lanelet::routing::RelationType::Successor)
            {
                return true;
            }
        }
        return false;
    }
    double RouteFollowingPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt)
    {
        lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = wm_->getTrafficRules();
        double target_speed;
        double hardcoded_max=lanelet::Velocity(hardcoded_params::control_limits::MAX_LONGITUDINAL_VELOCITY_MPS * lanelet::units::MPS()).value();

        if (traffic_rules)
        {
            target_speed=(*traffic_rules)->speedLimit(llt).speedLimit.value();

        }
        else
        {
            if(config_limit > 0 && config_limit < hardcoded_max)
            {
                target_speed=config_limit;
                ROS_WARN("Failed to set the current speed limit. Valid traffic rules object could not be built. Using Configurable value");
            }

            else
            {
                target_speed= hardcoded_max;
                ROS_WARN("Failed to set the current speed limit. Valid traffic rules object could not be built. Using Hardcoded maximum");
            }

        }
        return target_speed;
    }
}

max_stars_repo_name: harderthan/carma-platform

max_issues_repo_name: harderthan/carma-platform

max_forks_repo_name: harderthan/carma-platform


text:
#ifndef SOPUHS_TESTS_HPP
#define SOPUHS_TESTS_HPP

#include <Eigen/StdVector>
#include <unsupported/Eigen/MatrixFunctions>

#include <sophus/test_macros.hpp>

namespace Sophus {

template <class LieGroup>
class GenericTests {
 public:
  typedef typename LieGroup::Scalar Scalar;
  typedef typename LieGroup::Transformation Transformation;
  typedef typename LieGroup::Tangent Tangent;
  typedef typename LieGroup::Point Point;
  typedef typename LieGroup::Adjoint Adjoint;
  static const int N = LieGroup::N;
  static const int DoF = LieGroup::DoF;

  Scalar SMALL_EPS;

  GenericTests() : SMALL_EPS(Constants<Scalar>::epsilon()) {}

  void setGroupElements(
      const std::vector<LieGroup, Eigen::aligned_allocator<LieGroup>>&
          group_vec) {
    group_vec_ = group_vec;
  }

  void setTangentVectors(
      const std::vector<Tangent, Eigen::aligned_allocator<Tangent>>&
          tangent_vec) {
    tangent_vec_ = tangent_vec;
  }

  void setPoints(
      const std::vector<Point, Eigen::aligned_allocator<Point>>& point_vec) {
    point_vec_ = point_vec;
  }

  bool adjointTest() {
    bool passed = true;
    for (size_t i = 0; i < group_vec_.size(); ++i) {
      Transformation T = group_vec_[i].matrix();
      Adjoint Ad = group_vec_[i].Adj();
      for (size_t j = 0; j < tangent_vec_.size(); ++j) {
        Tangent x = tangent_vec_[j];

        Transformation I;
        I.setIdentity();
        Tangent ad1 = Ad * x;
        Tangent ad2 = LieGroup::vee(T * LieGroup::hat(x) *
                                    group_vec_[i].inverse().matrix());
        SOPHUS_TEST_APPROX(passed, ad1, ad2, 20. * SMALL_EPS,
                           "Adjoint case %, %", i, j);
      }
    }
    return passed;
  }

  bool expLogTest() {
    bool passed = true;

    for (size_t i = 0; i < group_vec_.size(); ++i) {
      Transformation T1 = group_vec_[i].matrix();
      Transformation T2 = LieGroup::exp(group_vec_[i].log()).matrix();
      SOPHUS_TEST_APPROX(passed, T1, T2, SMALL_EPS, "G - exp(log(G)) case: %",
                         i);
    }
    return passed;
  }

  bool expMapTest() {
    bool passed = true;
    for (size_t i = 0; i < tangent_vec_.size(); ++i) {
      Tangent omega = tangent_vec_[i];
      Transformation exp_x = LieGroup::exp(omega).matrix();
      Transformation expmap_hat_x = (LieGroup::hat(omega)).exp();
      SOPHUS_TEST_APPROX(passed, exp_x, expmap_hat_x, 10. * SMALL_EPS,
                         "expmap(hat(x)) - exp(x) case: %", i);
    }
    return passed;
  }

  bool groupActionTest() {
    bool passed = true;

    for (size_t i = 0; i < group_vec_.size(); ++i) {
      for (size_t j = 0; j < point_vec_.size(); ++j) {
        const Point& p = point_vec_[j];
        Transformation T = group_vec_[i].matrix();
        Point point1 = group_vec_[i] * p;
        Point point2 = map(T, p);
        SOPHUS_TEST_APPROX(passed, point1, point2, SMALL_EPS,
                           "Transform point case: %", i);
      }
    }
    return passed;
  }

  bool lieBracketTest() {
    bool passed = true;
    for (size_t i = 0; i < tangent_vec_.size(); ++i) {
      for (size_t j = 0; j < tangent_vec_.size(); ++j) {
        Tangent tangent1 =
            LieGroup::lieBracket(tangent_vec_[i], tangent_vec_[j]);
        Transformation hati = LieGroup::hat(tangent_vec_[i]);
        Transformation hatj = LieGroup::hat(tangent_vec_[j]);

        Tangent tangent2 = LieGroup::vee(hati * hatj - hatj * hati);
        SOPHUS_TEST_APPROX(passed, tangent1, tangent2, SMALL_EPS,
                           "Lie Bracket case: %", i);
      }
    }
    return passed;
  }

  bool veeHatTest() {
    bool passed = true;
    for (size_t i = 0; i < tangent_vec_.size(); ++i) {
      SOPHUS_TEST_APPROX(passed, tangent_vec_[i],
                         LieGroup::vee(LieGroup::hat(tangent_vec_[i])),
                         SMALL_EPS, "Hat-vee case: %", i);
    }
    return passed;
  }

  bool newDeleteSmokeTest() {
    bool passed = true;
    LieGroup* raw_ptr = nullptr;
    raw_ptr = new LieGroup();
    SOPHUS_TEST_NEQ(passed, reinterpret_cast<std::uintptr_t>(raw_ptr), 0);
    delete raw_ptr;
    return passed;
  }

  bool doAllTestsPass() {
    bool passed = adjointTest();
    passed &= expLogTest();
    passed &= expMapTest();
    passed &= groupActionTest();
    passed &= lieBracketTest();
    passed &= veeHatTest();
    passed &= newDeleteSmokeTest();
    return passed;
  }

 private:
  Eigen::Matrix<Scalar, N - 1, 1> map(
      const Eigen::Matrix<Scalar, N, N>& T,
      const Eigen::Matrix<Scalar, N - 1, 1>& p) {
    return T.template topLeftCorner<N - 1, N - 1>() * p +
           T.template topRightCorner<N - 1, 1>();
  }

  Eigen::Matrix<Scalar, N, 1> map(const Eigen::Matrix<Scalar, N, N>& T,
                                  const Eigen::Matrix<Scalar, N, 1>& p) {
    return T * p;
  }

  std::vector<LieGroup, Eigen::aligned_allocator<LieGroup>> group_vec_;
  std::vector<Tangent, Eigen::aligned_allocator<Tangent>> tangent_vec_;
  std::vector<Point, Eigen::aligned_allocator<Point>> point_vec_;
};
}
#endif  // TESTS_HPP

max_stars_repo_name: jpanikulam/experiments

max_issues_repo_name: jpanikulam/sonder

max_forks_repo_name: jpanikulam/sonder


text:
/*
 * GridTools
 *
 * Copyright (c) 2014-2019, ETH Zurich
 * All rights reserved.
 *
 * Please, refer to the LICENSE file in the root directory.
 * SPDX-License-Identifier: BSD-3-Clause
 */
#pragma once

#include <boost/preprocessor.hpp>

#include "common/function_traits.hpp"
#include "function_wrapper.hpp"
#include "generator.hpp"

#define GEN_EXPORT_BINDING_IMPL_PARAM_DECL(z, i, signature) \
    typename std::tuple_element<i,                          \
        ::cpp_bindgen::function_traits::parameter_types<::cpp_bindgen::wrapped_t<signature>>::type>::type param_##i

#define GEN_ADD_GENERATED_DEFINITION_IMPL(n, name, cppsignature, impl)                                            \
    static_assert(::cpp_bindgen::function_traits::arity<cppsignature>::value == n, "arity mismatch");             \
    extern "C" typename ::cpp_bindgen::function_traits::result_type<::cpp_bindgen::wrapped_t<cppsignature>>::type \
    name(BOOST_PP_ENUM(n, GEN_EXPORT_BINDING_IMPL_PARAM_DECL, cppsignature)) {                                    \
        return ::cpp_bindgen::wrap<cppsignature>(impl)(BOOST_PP_ENUM_PARAMS(n, param_));                          \
    }

/**
 *   Defines the function with the given name with the C linkage.
 *
 *   The signature if the generated function will be transformed from the provided signature according to the following
 *   rules:
 *     - for result type:
 *       - `void` and arithmetic types remain the same;
 *       - classes (and structures) and references to them are transformed to the pointer to the opaque handle
 *         (`gen_handle*`) which should be released by calling `void gen_release(gen_handle*)` function;
 *       - all other result types will cause a compiler error.
 *     - for parameter types:
 *       - arithmetic types and pointers to them remain the same;
 *       - references to arithmetic types are transformed to the corresponded pointers;
 *       - types that fulfill the concept of being fortran_array_bindable are transformed to a
 *         gen_fortran_array_descriptor
 *       - classes (and structures) and references or pointers to them are transformed to `gen_handle*`;
 *       - all other parameter types will cause a compiler error.
 *   Additionally the newly generated function will be registered for automatic interface generation.
 *
 *   @param n The arity of the generated function.
 *   @param name The name of the generated function.
 *   @param cppsignature The signature that will be used to invoke `impl`.
 *   @param impl The functor that the generated function will delegate to.
 */
#define GEN_EXPORT_BINDING_WITH_SIGNATURE(n, name, cppsignature, impl) \
    GEN_ADD_GENERATED_DEFINITION_IMPL(n, name, cppsignature, impl)     \
    GEN_ADD_GENERATED_DECLARATION(::cpp_bindgen::wrapped_t<cppsignature>, name)

/**
 *   Defines the function with the given name with the C linkage with an additional wrapper in the fortran bindings. The
 *   additional wrapper provides the following functionality:
 *     - It generates a gen_fortran_array_descriptor from an array, if the type on C++-side is fortran_array_wrappable.
 *
 *   The signature if the generated function will be transformed from the provided signature according to the following
 *   rules:
 *     - for result type:
 *       - `void` and arithmetic types remain the same;
 *       - classes (and structures) and references to them are transformed to the pointer to the opaque handle
 *         (`gen_handle*`) which should be released by calling `void gen_release(gen_handle*)` function;
 *       - all other result types will cause a compiler error.
 *     - for parameter types:
 *       - arithmetic types and pointers to them remain the same;
 *       - references to arithmetic types are transformed to the corresponded pointers;
 *       - types that are fortran_array_bindable but not fortran_array_wrappable are transformed to a
 *         gen_fortran_array_descriptor
 *       - types that are fortran_array_wrappable are transformed to a gen_fortran_array_descriptor in the c-bindings,
 *         and provide a wrapper in the fortran-bindings such that they can be called with a fortran array
 *       - classes (and structures) and references or pointers to them are transformed to `gen_handle*`;
 *       - all other parameter types will cause a compiler error.
 *   Additionally the newly generated function will be registered for automatic interface generation.
 *
 *   @param n The arity of the generated function.
 *   @param name The name of the generated function.
 *   @param cppsignature The signature that will be used to invoke `impl`.
 *   @param impl The functor that the generated function will delegate to.
 */
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(n, name, cppsignature, impl) \
    GEN_ADD_GENERATED_DEFINITION_IMPL(n, name, cppsignature, impl)             \
    GEN_ADD_GENERATED_DECLARATION_WRAPPED(cppsignature, name)

/// The flavour of GEN_EXPORT_BINDING_WITH_SIGNATURE where the `impl` parameter is a function pointer.
#define GEN_EXPORT_BINDING(n, name, impl) \
    GEN_EXPORT_BINDING_WITH_SIGNATURE(n, name, decltype(BOOST_PP_REMOVE_PARENS(impl)), impl)
#define GEN_EXPORT_BINDING_WRAPPED(n, name, impl) \
    GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(n, name, decltype(BOOST_PP_REMOVE_PARENS(impl)), impl)

#define GEN_EXPORT_GENERIC_BINDING_IMPL_IMPL(generatorsuffix, n, generic_name, concrete_name, impl) \
    BOOST_PP_CAT(GEN_EXPORT_BINDING, generatorsuffix)(n, concrete_name, impl);                      \
    GEN_ADD_GENERIC_DECLARATION(generic_name, concrete_name)

#define GEN_EXPORT_GENERIC_BINDING_IMPL(generatorsuffix, n, name, suffix, impl) \
    GEN_EXPORT_GENERIC_BINDING_IMPL_IMPL(generatorsuffix, n, name, BOOST_PP_CAT(name, suffix), impl)

#define GEN_EXPORT_GENERIC_BINDING_IMPL_FUNCTOR(r, data, i, elem)    \
    GEN_EXPORT_GENERIC_BINDING_IMPL(BOOST_PP_TUPLE_ELEM(4, 0, data), \
        BOOST_PP_TUPLE_ELEM(4, 1, data),                             \
        BOOST_PP_TUPLE_ELEM(4, 2, data),                             \
        i,                                                           \
        (BOOST_PP_TUPLE_ELEM(4, 3, data) < BOOST_PP_REMOVE_PARENS(elem) >));

#define GEN_EXPORT_GENERIC_BINDING(n, name, impl_template, template_params) \
    BOOST_PP_SEQ_FOR_EACH_I(GEN_EXPORT_GENERIC_BINDING_IMPL_FUNCTOR,        \
        (, n, name, impl_template),                                         \
        BOOST_PP_VARIADIC_SEQ_TO_SEQ(template_params))                      \
    static_assert(1, "")

#define GEN_EXPORT_GENERIC_BINDING_WRAPPED(n, name, impl_template, template_params) \
    BOOST_PP_SEQ_FOR_EACH_I(GEN_EXPORT_GENERIC_BINDING_IMPL_FUNCTOR,                \
        (_WRAPPED, n, name, impl_template),                                         \
        BOOST_PP_VARIADIC_SEQ_TO_SEQ(template_params))                              \
    static_assert(1, "")

/// GEN_EXPORT_BINDING_WITH_SIGNATURE shortcuts for the given arity
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_0(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(0, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_1(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(1, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_2(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(2, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_3(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(3, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_4(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(4, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_5(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(5, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_6(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(6, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_7(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(7, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_8(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(8, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_9(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE(9, name, s, i)

#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_0(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(0, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_1(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(1, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_2(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(2, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_3(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(3, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_4(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(4, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_5(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(5, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_6(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(6, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_7(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(7, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_8(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(8, name, s, i)
#define GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED_9(name, s, i) GEN_EXPORT_BINDING_WITH_SIGNATURE_WRAPPED(9, name, s, i)

/// GEN_EXPORT_BINDING shortcuts for the given arity
#define GEN_EXPORT_BINDING_0(name, impl) GEN_EXPORT_BINDING(0, name, impl)
#define GEN_EXPORT_BINDING_1(name, impl) GEN_EXPORT_BINDING(1, name, impl)
#define GEN_EXPORT_BINDING_2(name, impl) GEN_EXPORT_BINDING(2, name, impl)
#define GEN_EXPORT_BINDING_3(name, impl) GEN_EXPORT_BINDING(3, name, impl)
#define GEN_EXPORT_BINDING_4(name, impl) GEN_EXPORT_BINDING(4, name, impl)
#define GEN_EXPORT_BINDING_5(name, impl) GEN_EXPORT_BINDING(5, name, impl)
#define GEN_EXPORT_BINDING_6(name, impl) GEN_EXPORT_BINDING(6, name, impl)
#define GEN_EXPORT_BINDING_7(name, impl) GEN_EXPORT_BINDING(7, name, impl)
#define GEN_EXPORT_BINDING_8(name, impl) GEN_EXPORT_BINDING(8, name, impl)
#define GEN_EXPORT_BINDING_9(name, impl) GEN_EXPORT_BINDING(9, name, impl)

#define GEN_EXPORT_BINDING_WRAPPED_0(name, impl) GEN_EXPORT_BINDING_WRAPPED(0, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_1(name, impl) GEN_EXPORT_BINDING_WRAPPED(1, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_2(name, impl) GEN_EXPORT_BINDING_WRAPPED(2, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_3(name, impl) GEN_EXPORT_BINDING_WRAPPED(3, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_4(name, impl) GEN_EXPORT_BINDING_WRAPPED(4, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_5(name, impl) GEN_EXPORT_BINDING_WRAPPED(5, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_6(name, impl) GEN_EXPORT_BINDING_WRAPPED(6, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_7(name, impl) GEN_EXPORT_BINDING_WRAPPED(7, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_8(name, impl) GEN_EXPORT_BINDING_WRAPPED(8, name, impl)
#define GEN_EXPORT_BINDING_WRAPPED_9(name, impl) GEN_EXPORT_BINDING_WRAPPED(9, name, impl)