text:
#pragma once
#include <stdint.h>
#include <string>
#include <iostream>

#include <memory>
#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/spawn.hpp>

#include "base/thread/thread_pool.hpp"
#include "base/network/tcp_socket.hpp"

namespace mms {
template <typename CONN>
class ServerConnHandler {
public:
    virtual void onConnOpen(CONN *conn) {};
    virtual void onConnClosed(CONN *conn) {};
};

template <typename CONN>
class TcpServer {
public:
    TcpServer(ThreadWorker *worker):worker_(worker) {

    }

    virtual ~TcpServer() {

    }
public:
    void setConnHandler(ServerConnHandler<CONN> *handler) {
        handler_ = handler;
    }

    int32_t startListen(uint16_t port, const std::string & addr = "") {
        if (!worker_) {
            return -1;
        }

        boost::asio::spawn(worker_->getIOContext(), [port, addr, this](boost::asio::yield_context yield) {
            boost::asio::ip::tcp::endpoint endpoint;
            endpoint.port(port);
            if (!addr.empty()) {
                endpoint.address(boost::asio::ip::address::from_string(addr));
            } else {
                endpoint.address(boost::asio::ip::address::from_string("0.0.0.0"));
            }

            acceptor_ = boost::make_shared<boost::asio::ip::tcp::acceptor>(worker_->getIOContext(), endpoint);
            acceptor_->set_option(boost::asio::ip::tcp::acceptor::reuse_address(true));
            while(1) {
                boost::system::error_code ec;
                auto worker = thread_pool_inst::get_mutable_instance().getWorker(-1);
                boost::asio::ip::tcp::socket *tcp_sock = new boost::asio::ip::tcp::socket(worker->getIOContext());
                acceptor_->async_accept(*tcp_sock, yield[ec]);
                if (ec) {
                    delete tcp_sock;
                    tcp_sock = nullptr;
                    break;
                }

                boost::asio::spawn(worker->getIOContext(), [this, tcp_sock, worker](boost::asio::yield_context yield) {
                    auto client_conn = new CONN(tcp_sock, worker, yield);
                    handler_->onConnOpen(client_conn);
                });
            }
        });

        return 0;
    }

    void stopListen() {
        worker_->dispatch([this]{
            acceptor_->close();
            acceptor_.reset();
        });
    }
private:
    ThreadWorker *worker_;
    ServerConnHandler<CONN> *handler_;
    boost::shared_ptr<boost::asio::ip::tcp::acceptor> acceptor_;
};
};


text:
//
// Created by sc on 6/5/20.
//
#include "graph_drawer.h"
#include "../../Utils/define.h"
#include <map>
#include <vector>
#include <Eigen/Core>
#include <ORUtils/MathUtils.h>
//TODO: add label display on edges

using namespace PSLAM;

GraphDrawer::GraphDrawer():mbDrawNode(true), mbDrawEdge(true), mbDrawLabel(true),mbInited(false){

}

void GraphDrawer::Init(){
    mNodeDrawer.Init();
    mNodeDrawer.SetPointSize(0.1);
    mEdgeDrawer.Init();
    mTextDrawer.Init();
}

void GraphDrawer::Update(//float width, float height,
            const VecEigenf(3) &labelPositions,
            const VecEigenf(3) &labelColors,
            const std::vector<std::string> &texts,
            const std::vector<unsigned int> &edgeIndices
){
    mNodeDrawer.Update(&labelPositions,&labelColors);
    mEdgeDrawer.Update(&labelPositions, &edgeIndices);
    UpdateTextBuffer(labelPositions,labelColors,texts);
}

void GraphDrawer::Draw(float width, float height,
                       const Eigen::Matrix4f &view_matrix,
                       const Eigen::Matrix4f &projection){
    if(mbDrawNode)
        mNodeDrawer.Draw(projection, view_matrix);
    if(mbDrawLabel)
        DrawText(width,height,view_matrix,projection);
    if(mbDrawEdge)
        mEdgeDrawer.Draw(projection, view_matrix);
}

void GraphDrawer::UpdateTextBuffer(const VecEigenf(3) &labelPositions,
                                     const VecEigenf(3) &labelColors,
                                     const std::vector<std::string> &texts){
//            std::vector<std::pair<size_t,Eigen::Vector3f>, Eigen::aligned_allocator<std::pair<size_t, Eigen::Vector3f>>> labelPositionsOnImage(labelPositions.size());
    mTextBuffer.clear();
    mTextBuffer.reserve(labelPositions.size());
    for(size_t i=0;i<labelPositions.size();++i){
        mTextBuffer.emplace_back(TripletForSotring());
        auto &tripletForSotring = mTextBuffer.back();
        tripletForSotring.position3D = labelPositions[i];
        tripletForSotring.color    = labelColors[i];
        tripletForSotring.text      = texts[i];
    }
}

void GraphDrawer::DrawText(float windowWidth, float windowHeight, const Eigen::Matrix4f &view_matrix,
                           const Eigen::Matrix4f &projection){
    for(auto & buffer : mTextBuffer){
        const Eigen::Vector4f point (buffer.position3D.x(),buffer.position3D.y(),buffer.position3D.z(),1);
        Eigen::Vector4f projectedP = projection * view_matrix * point;
        projectedP.x() = projectedP.x() * windowWidth / 2 / projectedP.z() + windowWidth / 2; // the later windowHeight/ 2 should be cx
        projectedP.y() = projectedP.y() * windowHeight/ 2 / projectedP.z() + windowHeight/ 2;
        buffer.position = Eigen::Vector3f(projectedP.x(),projectedP.y(),projectedP.z());
    }
    // order them so the back text will not block the front
    std::sort(mTextBuffer.begin(), mTextBuffer.end(),
              [](const TripletForSotring &left, const TripletForSotring &right){
                  return left.position.z() < right.position.z();
              });
    for(auto & i : mTextBuffer){
        const auto &point = i.position;
        const auto &color = i.color;
        auto text = i.text;
        if(point.z() < 0) continue;
//        std::cout << text << ": " << point.z() << "\n";
        float scale = CLAMP((5-std::sqrt(point.z()))/5.f,0.1f,1.f);
        mTextDrawer.Draw(text,
                         point.x(),point.y(), float(windowWidth), float(windowHeight),scale, //TODO: add a controllabel scale here
                         {color.x(),color.y(),color.z()});
    }
}

ext: cpp

max_stars_repo_name: jkabalar/SceneGraphFusion

max_issues_repo_name: jkabalar/SceneGraphFusion

max_forks_repo_name: jkabalar/SceneGraphFusion


text:
/**
 * The MIT License (MIT)
 *
 * Copyright © 2018-2019 Ruben Van Boxem
 *
 * 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.
 **/

/*
 * css::grammar
 * Rules are based on description at https://www.w3schools.com/css/css_syntax.asp
 */

#ifndef SKUI_CSS_GRAMMAR_H
#define SKUI_CSS_GRAMMAR_H

#include "css/declaration_block.h++"
#include "css/property.h++"
#include "css/rule_set.h++"

#include "css/grammar/align_content.h++"
#include "css/grammar/align_items.h++"
#include "css/grammar/align_self.h++"
#include "css/grammar/animation.h++"
#include "css/grammar/background.h++"
#include "css/grammar/backface_visibility.h++"
#include "css/grammar/color.h++"
#include "css/grammar/x3_stdvariant.h++"

#include "css/grammar/make_property.h++"

#include <boost/spirit/home/x3.hpp>

#include <unordered_map>

namespace skui::css::grammar
{
  namespace x3 = boost::spirit::x3;

  const auto declaration = make_property("align-content", align_content, &css::declaration_block::align_content)
                         | make_property("align-items", align_items, &css::declaration_block::align_items)
                         | make_property("align-self", align_self, &css::declaration_block::align_self)
                      // | make_property("all", all, &css::declaration_block::all)
                         | make_property("animation", animation, &css::declaration_block::animation)
                         | make_property("animation-delay", animation_delay, &css::declaration_block::animation, &css::animation::delay)
                         | make_property("animation-direction", animation_direction, &css::declaration_block::animation, &css::animation::direction)
                         | make_property("animation-duration", animation_duration, &css::declaration_block::animation, &css::animation::duration)
                         | make_property("animation-fill-mode", animation_fill_mode, &css::declaration_block::animation, &css::animation::fill_mode)
                         | make_property("animation-iteration-count", animation_iteration_count, &css::declaration_block::animation, &css::animation::iteration_count)
                         | make_property("animation-name", animation_name, &css::declaration_block::animation, &css::animation::name)
                         | make_property("animation-play-state", animation_play_state, &css::declaration_block::animation, &css::animation::play_state)
                         | make_property("animation-timing-function", animation_timing_function, &css::declaration_block::animation, &css::animation::timing_function)
                         | make_property("backface-visibility", backface_visibility, &css::declaration_block::backface_visibility)
                         | make_property("background", background, &css::declaration_block::background)
                         | make_background_property("background-color", color, &css::background::color)
                         | make_background_property("background-image", background_image, &css::background::image)
                         | make_background_property("background-position", background_position, &css::background::position)
                         | make_background_property("background-size", background_size, &css::background::size)
                         | make_background_property("background-repeat", background_repeat, &css::background::repeat)
                         | make_background_property("background-origin", background_origin, &css::background::origin)
                         | make_background_property("background-clip", background_clip, &css::background::clip)
                         | make_background_property("background-attachment", background_attachment, &css::background::attachment)
                         ;

  const auto declaration_block = x3::rule<struct declaration_block, css::declaration_block>{"declaration_block"}
                               = '{' >> +declaration >> '}';
}

#endif

max_stars_repo_name: rubenvb/skui

max_issues_repo_name: rubenvb/skui

max_forks_repo_name: rubenvb/skui


text:
#include "HDBlock.h"

#include <boost/endian/buffers.hpp>

namespace mdf {

#pragma pack(push, 1)
  struct HDBlockData {
    boost::endian::little_uint64_buf_t start_time_ns;
    boost::endian::little_int16_buf_t tz_offset_min;
    boost::endian::little_int16_buf_t dst_offset_min;
    boost::endian::little_uint8_buf_t time_flags;
    boost::endian::little_uint8_buf_t time_class;
    boost::endian::little_uint8_buf_t flags;
    boost::endian::little_uint8_buf_t reserved;
    boost::endian::little_float64_buf_t start_angle_rad;
    boost::endian::little_float64_buf_t start_distance_m;
  };
#pragma pack(pop)

  bool HDBlock::load(uint8_t const *dataPtr) {
    bool result = false;

    // Load data into a struct for easier access.
    auto ptr = reinterpret_cast<HDBlockData const *>(dataPtr);

    startTimeNs = ptr->start_time_ns.value();
    tzOffsetMin = ptr->tz_offset_min.value();
    dstOffsetMin = ptr->dst_offset_min.value();
    timeFlags = ptr->time_flags.value();
    timeClass = ptr->time_class.value();
    flags = ptr->flags.value();
    start_angle_rad = ptr->start_angle_rad.value();
    start_distance_m = ptr->start_distance_m.value();

    result = true;

    return result;
  }

  std::shared_ptr<DGBlock> HDBlock::getFirstDGBlock() const {
    return std::dynamic_pointer_cast<DGBlock>(links[0]);
  }

  std::shared_ptr<MDBlock> HDBlock::getComment() const {
    return std::dynamic_pointer_cast<MDBlock>(links[5]);
  }

  bool HDBlock::saveBlockData(uint8_t *dataPtr) {
    auto ptr = reinterpret_cast<HDBlockData *>(dataPtr);
    ptr->start_time_ns = startTimeNs;
    ptr->tz_offset_min = tzOffsetMin;
    ptr->dst_offset_min = dstOffsetMin;
    ptr->time_flags = timeFlags;
    ptr->time_class = timeClass;
    ptr->flags = flags;
    ptr->start_angle_rad = start_angle_rad;
    ptr->start_distance_m = start_distance_m;

    return true;
  }

  uint64_t HDBlock::getStartTimeNs() const {
    return startTimeNs;
  }

  int16_t HDBlock::getTzOffsetMin() const {
    return tzOffsetMin;
  }

  int16_t HDBlock::getDstOffsetMin() const {
    return dstOffsetMin;
  }

}


text: #include <boost/fusion/include/replace.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*
 * Config.cpp
 *
 *  Created on: 13 Feb 2015
 *      Author: rian
 */

#include "Config.h"
#include <iostream>
#include <fstream>
#include <string>
#include <boost/log/core.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/expressions.hpp>
#include "../../contrib/jsoncpp/reader.h"

namespace raytracer {
namespace core {

	using namespace std;

	Config::Config() {}

	Config::Config(const char * filepath) {

		loadFromFile(filepath);
	}

	void Config::loadFromFile(const char * filepath) {

		Json::Reader r = Json::Reader();
		std::ifstream test(filepath, std::ifstream::binary);
		bool success = r.parse(test, _doc, false);
		if ( !success ) {
			// report to the user the failure and their locations in the document.
			BOOST_LOG_TRIVIAL(fatal) << "Formatting error in config file "
					<< filepath
					<< "!\n" << r.getFormattedErrorMessages();
			std::exit(0);
		}
	}

	int Config::getInt(const char * path) {

		if (!_doc.isMember(path)) {
			cerr << path << " does not exist!" << endl;
		}

		return _doc.get(path, "").asInt();
	}

	double Config::getDouble(const char * path) {

		if (!_doc.isMember(path)) {
			cerr << path << " does not exist!" << endl;
		}

		return atof(_doc.get(path, "").asCString());
	}

	Json::Value Config::getArray(const char * path) {

		if (!_doc.isMember(path)) {
			cerr << path << " does not exist!" << endl;
		}

		if (!_doc.get(path, "").isArray()) {
			cerr << path << " is not an array!" << endl;
		}

		return _doc[path];
	}

	Json::Value Config::getObject(const char * path) {

		if (!_doc.isMember(path)) {
			cerr << path << " does not exist!" << endl;
		}

		if (!_doc.get(path, "").isObject()) {
			cerr << path << " is not an object!" << endl;
		}

		return _doc[path];
	}

} /* namespace core */
} /* namespace raytracer */

max_stars_repo_name: rvangijlswijk/ionospheric-ray-tracer


text:
#ifndef MYSQL_ASIO_IMPL_SERIALIZATION_HPP
#define MYSQL_ASIO_IMPL_SERIALIZATION_HPP

#include <boost/endian/conversion.hpp>
#include <type_traits>
#include <algorithm>
#include "boost/mysql/detail/protocol/protocol_types.hpp"
#include "boost/mysql/detail/protocol/serialization_context.hpp"
#include "boost/mysql/detail/protocol/deserialization_context.hpp"
#include "boost/mysql/detail/auxiliar/bytestring.hpp"
#include "boost/mysql/value.hpp"
#include "boost/mysql/error.hpp"

namespace boost {
namespace mysql {
namespace detail {

enum class serialization_tag
{
	none,
	fixed_size_int,
	floating_point,
	enumeration,
	struct_with_fields
};

template <typename T>
constexpr serialization_tag get_serialization_tag();

template <typename T, serialization_tag=get_serialization_tag<T>()>
struct serialization_traits;

template <typename T>
errc deserialize(T& output, deserialization_context& ctx) noexcept
{
	return serialization_traits<T>::deserialize_(output, ctx);
}

template <typename T>
void serialize(const T& input, serialization_context& ctx) noexcept
{
	serialization_traits<T>::serialize_(input, ctx);
}

template <typename T>
std::size_t get_size(const T& input, const serialization_context& ctx) noexcept
{
	return serialization_traits<T>::get_size_(input, ctx);
}

// Fixed-size integers
template <typename T>
constexpr bool is_fixed_size_int();

template <typename T>
struct serialization_traits<T, serialization_tag::fixed_size_int>
{
	static errc deserialize_(T& output, deserialization_context& ctx) noexcept;
	static void serialize_(T input, serialization_context& ctx) noexcept;
	static constexpr std::size_t get_size_(T, const serialization_context&) noexcept;
};

// int_lenenc
template <>
struct serialization_traits<int_lenenc, serialization_tag::none>
{
	static inline errc deserialize_(int_lenenc& output, deserialization_context& ctx) noexcept;
	static inline void serialize_(int_lenenc input, serialization_context& ctx) noexcept;
	static inline std::size_t get_size_(int_lenenc input, const serialization_context&) noexcept;
};

// string_fixed
template <std::size_t N>
struct serialization_traits<string_fixed<N>, serialization_tag::none>
{
	static errc deserialize_(string_fixed<N>& output, deserialization_context& ctx) noexcept;
	static void serialize_(const string_fixed<N>& input, serialization_context& ctx) noexcept
	{
		ctx.write(input.data(), N);
	}
	static constexpr std::size_t get_size_(const string_fixed<N>&, const serialization_context&) noexcept
	{
		return N;
	}
};

// string_null
template <>
struct serialization_traits<string_null, serialization_tag::none>
{
	static inline errc deserialize_(string_null& output, deserialization_context& ctx) noexcept;
	static inline void serialize_(string_null input, serialization_context& ctx) noexcept
	{
		ctx.write(input.value.data(), input.value.size());
		ctx.write(0); // null terminator
	}
	static inline std::size_t get_size_(string_null input, const serialization_context&) noexcept
	{
		return input.value.size() + 1;
	}
};

// string_eof
template <>
struct serialization_traits<string_eof, serialization_tag::none>
{
	static inline errc deserialize_(string_eof& output, deserialization_context& ctx) noexcept;
	static inline void serialize_(string_eof input, serialization_context& ctx) noexcept
	{
		ctx.write(input.value.data(), input.value.size());
	}
	static inline std::size_t get_size_(string_eof input, const serialization_context&) noexcept
	{
		return input.value.size();
	}
};

// string_lenenc
template <>
struct serialization_traits<string_lenenc, serialization_tag::none>
{
	static inline errc deserialize_(string_lenenc& output, deserialization_context& ctx) noexcept;
	static inline void serialize_(string_lenenc input, serialization_context& ctx) noexcept
	{
		serialize(int_lenenc(input.value.size()), ctx);
		ctx.write(input.value.data(), input.value.size());
	}
	static inline std::size_t get_size_(string_lenenc input, const serialization_context& ctx) noexcept
	{
		return get_size(int_lenenc(input.value.size()), ctx) + input.value.size();
	}
};

// Enums
template <typename T>
struct serialization_traits<T, serialization_tag::enumeration>
{
	using underlying_type = std::underlying_type_t<T>;
	using serializable_type = value_holder<underlying_type>;

	static errc deserialize_(T& output, deserialization_context& ctx) noexcept;
	static void serialize_(T input, serialization_context& ctx) noexcept
	{
		serialize(serializable_type(static_cast<underlying_type>(input)), ctx);
	}
	static std::size_t get_size_(T, const serialization_context&) noexcept;
};

// Floating points
template <typename T>
struct serialization_traits<T, serialization_tag::floating_point>
{
	static_assert(std::numeric_limits<T>::is_iec559);
	static errc deserialize_(T& output, deserialization_context& ctx) noexcept;
	static void serialize_(T input, serialization_context& ctx) noexcept;
	static std::size_t get_size_(T, const serialization_context&) noexcept
	{
		return sizeof(T);
	}
};

// Dates and times
template <>
struct serialization_traits<date, serialization_tag::none>
{
	static inline std::size_t get_size_(const date& input, const serialization_context& ctx) noexcept;
	static inline void serialize_(const date& input, serialization_context& ctx) noexcept;
	static inline errc deserialize_(date& output, deserialization_context& ctx) noexcept;
};

template <>
struct serialization_traits<datetime, serialization_tag::none>
{
	static inline std::size_t get_size_(const datetime& input, const serialization_context& ctx) noexcept;
	static inline void serialize_(const datetime& input, serialization_context& ctx) noexcept;
	static inline errc deserialize_(datetime& output, deserialization_context& ctx) noexcept;
};

template <>
struct serialization_traits<time, serialization_tag::none>
{
	static inline std::size_t get_size_(const time& input, const serialization_context& ctx) noexcept;
	static inline void serialize_(const time& input, serialization_context& ctx) noexcept;
	static inline errc deserialize_(time& output, deserialization_context& ctx) noexcept;
};

// Structs and commands (messages)
// To allow a limited way of reflection, structs should
// specialize get_struct_fields with a tuple of pointers to members,
// thus defining which fields should be (de)serialized in the struct
// and in which order
struct not_a_struct_with_fields {}; // Tag indicating a type is not a struct with fields

template <typename T>
struct get_struct_fields
{
	static constexpr not_a_struct_with_fields value {};
};

template <typename T>
constexpr bool is_struct_with_fields();

template <typename T>
struct serialization_traits<T, serialization_tag::struct_with_fields>
{
	static errc deserialize_(T& output, deserialization_context& ctx) noexcept;
	static void serialize_(const T& input, serialization_context& ctx) noexcept;
	static std::size_t get_size_(const T& input, const serialization_context& ctx) noexcept;
};

// Use these to make all messages implement all methods, leaving the not required
// ones with a default implementation. While this is not strictly required,
// it simplifies the testing infrastructure
template <typename T>
struct noop_serialize
{
	static inline std::size_t get_size_(const T&, const serialization_context&) noexcept { return 0; }
	static inline void serialize_(const T&, serialization_context&) noexcept {}
};

template <typename T>
struct noop_deserialize
{
	static inline errc deserialize_(T&, deserialization_context&) noexcept { return errc::ok; }
};

// Helper to serialize top-level messages
template <typename Serializable, typename Allocator>
void serialize_message(
	const Serializable& input,
	capabilities caps,
	basic_bytestring<Allocator>& buffer
);

template <typename Deserializable>
error_code deserialize_message(
	Deserializable& output,
	deserialization_context& ctx
);

// Helpers for (de) serializing a set of fields
template <typename... Types>
errc deserialize_fields(deserialization_context& ctx, Types&... fields) noexcept;

template <typename... Types>
void serialize_fields(serialization_context& ctx, const Types&... fields) noexcept;

inline std::pair<error_code, std::uint8_t> deserialize_message_type(
	deserialization_context& ctx
);

} // detail
} // mysql
} // boost

#include "boost/mysql/detail/protocol/impl/serialization.hpp"
#include "boost/mysql/detail/protocol/impl/serialization.ipp"

#endif


text:
#define _WIN32_WINNT 0x0501

#include "sockets.hpp"
#include <cstring>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fcntl.h>
#include "types.hpp"
#include <boost/thread/recursive_mutex.hpp>
#include "unicode/unistr.h"

#ifdef _WIN32
    #ifndef __WIN32
    #define __WIN32
    #endif
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#include <winsock2.h>
#include <ws2tcpip.h>
#else
#include <unistd.h>
#endif

using namespace trankesbel;
using namespace std;
using namespace boost;

#ifdef _WIN32
static bool winsock_initialized = false;
static recursive_mutex winsock_initialized_mutex;

static void second_wait(int seconds)
{
    Sleep(seconds * 1000);
}

static void test_initialize_winsock()
{
    lock_guard<recursive_mutex> lock(winsock_initialized_mutex);

    if (!winsock_initialized)
    {
        WSAData wd;
        if (WSAStartup(MAKEWORD(2, 2), &wd) != 0)
        {
            cerr << "WinSock initialization failed!" << endl;
            terminate();
            return;
        }
        winsock_initialized = true;
    }
}

static void test_winsock_shutdown()
{
    lock_guard<recursive_mutex> lock(winsock_initialized_mutex);

    if (winsock_initialized)
    {
        WSACleanup();
        winsock_initialized = false;
    }
}
#else
static void second_wait(int seconds)
{
    sleep(seconds);
}
static void test_winsock_shutdown() { };
static void test_initialize_winsock() { };
#endif

SocketAddress::SocketAddress()
{
    ip_protocol = IPv4;
    memset(&sa4, 0, sizeof(sa4));
    memset(&sa6, 0, sizeof(sa6));

    sa4.sin_family = PF_INET;
    sa6.sin6_family = PF_INET6;
}

SocketAddress::~SocketAddress()
{
    lock_guard<recursive_mutex> hostname_lock(hostname_mutex);
}

SocketAddress::SocketAddress(const SocketAddress &sa)
{
    memcpy(&sa4, &sa.sa4, sizeof(sa4));
    memcpy(&sa6, &sa.sa6, sizeof(sa6));
    ip_protocol = sa.ip_protocol;

    SocketAddress* foreign = const_cast<SocketAddress*>(&sa);

    lock_guard<recursive_mutex> hostname_lock(foreign->hostname_mutex);
    hostname = sa.hostname;
}

SocketAddress& SocketAddress::operator=(const SocketAddress &sa)
{
    if (this == &sa) return (*this);

    memcpy(&sa4, &sa.sa4, sizeof(sa4));
    memcpy(&sa6, &sa.sa6, sizeof(sa6));
    ip_protocol = sa.ip_protocol;

    SocketAddress* foreign = const_cast<SocketAddress*>(&sa);

    lock_guard<recursive_mutex> hostname_own_lock(hostname_mutex);
    lock_guard<recursive_mutex> hostname_lock(foreign->hostname_mutex);
    hostname = sa.hostname;

    return (*this);
}

SocketAddress::SocketAddress(const struct sockaddr_in* sai)
{
    ip_protocol = IPv4;
    memcpy(&sa4, sai, sizeof(sa4));
    memset(&sa6, 0, sizeof(sa6));

    sa4.sin_family = PF_INET;
    sa6.sin6_family = PF_INET6;
}

SocketAddress::SocketAddress(const struct sockaddr_in6* sai6)
{
    ip_protocol = IPv6;
    memset(&sa4, 0, sizeof(sa4));
    memcpy(&sa6, sai6, sizeof(sa6));

    sa4.sin_family = PF_INET;
    sa6.sin6_family = PF_INET6;
}

bool SocketAddress::operator==(const SocketAddress &sa) const
{
    if (ip_protocol != sa.ip_protocol) return false;

    if (ip_protocol == IPv4)
        if (memcmp(&sa.sa4, &sa4, sizeof(sa4)))
            return false;
    if (ip_protocol == IPv6)
        if (memcmp(&sa.sa6, &sa6, sizeof(sa6)))
            return false;

    if (sa.hostname != hostname)
        return false;

    return true;
}

bool SocketAddress::operator<(const SocketAddress &sa) const
{
    if (ip_protocol == IPv4 && sa.ip_protocol == IPv6) return true;
    if (ip_protocol == IPv6 && sa.ip_protocol == IPv4) return false;

    char* b1, *b2;
    size_t b_size;
    if (ip_protocol == IPv4)
    {
        b1 = (char*) &sa4;
        b2 = (char*) &sa.sa4;
        b_size = sizeof(sa4);
    }
    else
    {
        b1 = (char*) &sa6;
        b2 = (char*) &sa.sa6;
        b_size = sizeof(sa6);
    }

    size_t i1;
    for (i1 = 0; i1 < b_size; ++i1)
    {
        if (b1[i1] == b2[i1]) continue;

        if (b1[i1] < b2[i1]) return true;

        return false;
    }

    return hostname < sa.hostname;
}

string SocketAddress::serialize() const
{
    stringstream ss;

    /* First byte is either 4 or 6, for IPvX versions */
    if (ip_protocol == IPv4)
    {
        ss << "4";
        ui16 p = getPort();
        ss << (unsigned char) (p >> 8);
        ss << (unsigned char) (p & 0x00ff);

        ui32 address = ntohl(sa4.sin_addr.s_addr);
        ss << (unsigned char) ( (address & 0xff000000) >> 24 );
        ss << (unsigned char) ( (address & 0x00ff0000) >> 16 );
        ss << (unsigned char) ( (address & 0x0000ff00) >> 8 );
        ss << (unsigned char) ( (address & 0x000000ff) );

        ss << hostname;

        return ss.str();
    }

    ss << "6";
    ui16 p = getPort();
    ss << (unsigned char) (p >> 8);
    ss << (unsigned char) (p & 0x00ff);

    const char* buf = (const char*) &sa6.sin6_addr;
    for (ui32 i1 = 0; i1 < 16; i1++)
        ss << buf[i1];

    ss << hostname;

    return ss.str();
}

bool SocketAddress::unSerialize(const std::string &s)
{
    const char* c = s.c_str();
    size_t len = s.size();

    if (c[0] == '4') /* IPv4 address */
    {
        if (len < 7) return false; /* Invalid data */

        ip_protocol = IPv4;

        ui16 p = 0;
        p = (ui16) c[1] << 8;
        p |= (ui16) c[2];

        ui32 address = 0;
        address = (ui32) c[3] << 24;
        address |= (ui32) c[4] << 16;
        address |= (ui32) c[5] << 8;
        address |= (ui32) c[6];

        setPort(p);
        sa4.sin_addr.s_addr = htonl(address);

        hostname = s.substr(7);

        memset(&sa6, 0, sizeof(sa6));
        return true;
    }
    if (c[0] == '6') /* IPv6 address */
    {
        if (len < 19) return false; /* invalid data */

        ip_protocol = IPv6;

        ui16 p = 0;
        p = (ui16) c[1] << 8;
        p |= (ui16) c[2];

        setPort(p);

        for (ui32 i1 = 3; i1 < 19; i1++)
            ((char*) &sa6.sin6_addr)[i1-3] = c[i1];

        hostname = s.substr(19);
        memset(&sa4, 0, sizeof(sa4));
        return true;
    }

    return false;
}

struct address_info_s
{
   struct sockaddr_in sa4;
   struct sockaddr_in6 sa6;
   function3<void, bool, string, string> signal_function;
   ProtocolFamily fam;

   address_info_s() { };
};

#ifdef _WIN32
static int getSocketError() { return WSAGetLastError(); };
static int getEWOULDBLOCK() { return WSAEWOULDBLOCK; };
static void resetSocketError() { WSASetLastError(0); };

static string getErrorStringSocketsError(int error_code)
{
    LPWSTR buffer_string = (LPWSTR) 0;
    if (!FormatMessageW(FORMAT_MESSAGE_ALLOCATE_BUFFER|FORMAT_MESSAGE_FROM_SYSTEM, NULL, error_code, 0, (LPWSTR) &buffer_string, 1, 0))
        return string("");

    if (!buffer_string)
        return string("");

    return TO_UTF8(buffer_string);
#else
static int getSocketError() { return errno; };
static int getEWOULDBLOCK() { return EWOULDBLOCK; };
static void resetSocketError() { errno = 0; };

static string getErrorStringSocketsError(int error_code)
{
    char node_buf[501];
    node_buf[500] = 0;
    node_buf[0] = 0;
#ifdef _GNU_SOURCE
    /* GNU strerror_r */
    return strerror_r(error_code, node_buf, 500);
#else
    strerror_r(error_code, node_buf, 500);
    return string(node_buf);
#endif

#endif
}

static string getErrorStringEAI(int errorcode)
{
    char node_buf[501];
    node_buf[500] = 0;

    switch(errorcode)
    {
        case EAI_AGAIN:
            return "Could not resolve the name before timeout. (EAI_AGAIN)";
        break;
        case EAI_BADFLAGS:
            return "Bad flags. (EAI_BADFLAGS)";
        break;
        case EAI_FAIL:
            return "Non-recoverable error occured. (EAI_FAIL)";
        break;
        case EAI_FAMILY:
            return "Unknown address family. (EAI_FAMILY)";
        break;
        case EAI_MEMORY:
            return "Memory allocation failure. (EAI_MEMORY)";
        break;
        case EAI_NONAME:
            return "Name doesn't resolve to asked format. (EAI_NONAME)";
        break;
#ifndef _WIN32
        case EAI_OVERFLOW:
            return "Argument overflow. (EAI_OVERFLOW)";
        break;
        case EAI_SYSTEM:
            return getErrorStringSocketsError(errno);
            node_buf[0] = 0;
#ifdef _GNU_SOURCE
            return strerror_r(errno, node_buf, 500);
#else
            strerror_r(errno, node_buf, 500);
            return string(node_buf);
#endif
        break;
#endif
        default:
#ifdef _WIN32
            return getErrorStringSocketsError(WSAGetLastError());
#else
            return "Unknown error.";
#endif
        break;
    };
}

void SocketAddress::getHumanReadable_thread(void* address_info)
{
    SP<struct address_info_s> ais((struct address_info_s*) address_info);
    char node_buf[501];
    char service_buf[501];
    /* It should not be possible to reach 500th byte with getnameinfo(),
       but I felt like having some insurance. */
    node_buf[500] = 0;
    service_buf[500] = 0;
    int result;

    int max_wait = 20;
    do
    {
        if (ais->fam == IPv4)
            result = getnameinfo((struct sockaddr*) &ais->sa4, sizeof(ais->sa4), node_buf, 500, service_buf, 500, NI_NUMERICHOST|NI_NUMERICSERV);
        else
            result = getnameinfo((struct sockaddr*) &ais->sa6, sizeof(ais->sa6), node_buf, 500, service_buf, 500, NI_NUMERICHOST|NI_NUMERICSERV);

        if (result == EAI_AGAIN)
            second_wait(1);
        else
            break;
    }
    while( (max_wait--) > 0);

    if (!result)
    {
        ais->signal_function(true, string(node_buf), string(service_buf));
        return;
    }

    /* gai_strerror is not thread-safe and we don't want to be windows-specific,
       therefore, we must use our own error messages. */
    ais->signal_function(false, getErrorStringEAI(result), "");
}

string SocketAddress::getHumanReadablePlainUTF8_both(bool with_port) const
{
    char node_buf[501];
    char service_buf[501];
    node_buf[500] = 0;
    service_buf[500] = 0;
    int result;

    address_info_s ais;
    memcpy(&ais.sa4, &sa4, sizeof(sa4));
    memcpy(&ais.sa6, &sa6, sizeof(sa6));
    ais.fam = ip_protocol;

    if (ais.fam == IPv4)
        result = getnameinfo((struct sockaddr*) &ais.sa4, sizeof(ais.sa4), node_buf, 500, service_buf, 500, NI_NUMERICHOST|NI_NUMERICSERV);
    else
        result = getnameinfo((struct sockaddr*) &ais.sa6, sizeof(ais.sa6), node_buf, 500, service_buf, 500, NI_NUMERICHOST|NI_NUMERICSERV);
    if (result != 0) return string("");

    if (with_port)
    {
        if (ais.fam == IPv4)
        {
            stringstream ss;
            ss << node_buf << ":" << service_buf;
            return ss.str();
        }
        else
        {
            stringstream ss;
            ss << "[" << node_buf << "]:" << service_buf;
            return ss.str();
        }
    }

    return string(node_buf);
}

void SocketAddress::getHumanReadable(function3<void, bool, string, string> humanreadable_signal, bool block)
{
    if (!humanreadable_signal) return;

    address_info_s* ais = new address_info_s;
    memset(ais, 0, sizeof(*ais));

    memcpy(&ais->sa4, &sa4, sizeof(sa4));
    memcpy(&ais->sa6, &sa6, sizeof(sa6));
    ais->signal_function = humanreadable_signal;
    ais->fam = ip_protocol;

    thread thread_lookup(getHumanReadable_thread, (void*) ais);
    if (block)
        thread_lookup.join();
    else
        thread_lookup.detach();
}

void SocketAddress::resolve_thread(string address_string, string service_string, function3<void, bool, SocketAddress, string> readysignal)
{
    struct addrinfo *ai = (struct addrinfo*) 0;
    int result = getaddrinfo(address_string.c_str(), service_string.c_str(), NULL, &ai);
    if (result)
    {
        if (ai) freeaddrinfo(ai);
        readysignal(false, SocketAddress(), getErrorStringEAI(result));
        return;
    }

    struct addrinfo *i1;
    for (i1 = ai; i1; i1 = i1->ai_next)
    {
        if (i1->ai_family != PF_INET &&
            i1->ai_family != PF_INET6)
            continue;

        if (i1->ai_addr)
        {
            if (i1->ai_family == PF_INET)
            {
                SocketAddress sa;
                sa.ip_protocol = IPv4;
                memcpy(&sa.sa4, i1->ai_addr, sizeof(sa.sa4));
                sa.sa4.sin_family = PF_INET;
                readysignal(true, sa, "");
                freeaddrinfo(ai);
                return;
            }
            if (i1->ai_family == PF_INET6)
            {
                SocketAddress sa;
                sa.ip_protocol = IPv6;
                memcpy(&sa.sa6, i1->ai_addr, sizeof(sa.sa6));
                sa.sa6.sin6_family = PF_INET6;
                readysignal(true, sa, "");
                freeaddrinfo(ai);
                return;
            }
        }
    };

    freeaddrinfo(ai);
    readysignal(false, SocketAddress(), "Could not find supported address for asked name.");
}

static void resolve_callback(bool success, SocketAddress sa, string errormsg, bool* success2, SocketAddress* sa2, string* errormsg2)
{
    (*success2) = success;
    (*errormsg2) = errormsg;
    (*sa2) = sa;
}

SocketAddress SocketAddress::resolvePlainUTF8(std::string address_string, std::string service_string, bool* success, std::string* errormsg)
{
    struct addrinfo *ai = (struct addrinfo*) 0;
    struct addrinfo hints;
    memset(&hints, 0, sizeof(struct addrinfo));
    hints.ai_family = AF_UNSPEC;
    hints.ai_flags = AI_NUMERICHOST;

    int result = getaddrinfo(address_string.c_str(), service_string.c_str(), &hints, &ai);
    if (result)
    {
        if (ai) freeaddrinfo(ai);
        if (success) (*success) = false;
        if (errormsg) (*errormsg) = getErrorStringEAI(result);

        return SocketAddress();
    }

    struct addrinfo *i1;
    for (i1 = ai; i1; i1 = i1->ai_next)
    {
        if (i1->ai_family != PF_INET &&
            i1->ai_family != PF_INET6)
            continue;

        if (i1->ai_addr)
        {
            if (i1->ai_family == PF_INET)
            {
                SocketAddress sa;
                sa.ip_protocol = IPv4;
                memcpy(&sa.sa4, i1->ai_addr, sizeof(sa.sa4));
                sa.sa4.sin_family = PF_INET;
                if (success) (*success) = true;

                freeaddrinfo(ai);
                return sa;
            }
            if (i1->ai_family == PF_INET6)
            {
                SocketAddress sa;
                sa.ip_protocol = IPv6;
                memcpy(&sa.sa6, i1->ai_addr, sizeof(sa.sa6));
                sa.sa6.sin6_family = PF_INET6;
                if (success) (*success) = true;

                freeaddrinfo(ai);
                return sa;
            }
        }
    };

    freeaddrinfo(ai);

    if (success) (*success) = false;
    if (errormsg) (*errormsg) = "Could not find supported address for asked name.";

    return SocketAddress();
}

SocketAddress SocketAddress::resolveUTF8(string address_string, string service_string, bool* success2, string* errormsg2)
{
    string result_str;
    bool success = false;
    SocketAddress sa;

    function3<void, bool, SocketAddress, string> callback;
    callback = boost::bind(resolve_callback, _1, _2, _3, &success, &sa, &result_str);

    resolve(address_string, service_string, callback, true);

    if (errormsg2) (*errormsg2) = result_str;
    if (success2) (*success2) = success;

    return sa;
}

void SocketAddress::resolve(string address_string, string service_string, function3<void, bool, SocketAddress, string> readysignal, bool block)
{
    thread socketaddress_creation(resolve_thread, address_string, service_string, readysignal);
    if (block)
        socketaddress_creation.join();
    else
        socketaddress_creation.detach();
}

ui16 SocketAddress::getPort() const
{
    if (ip_protocol == IPv4)
        return ntohs(sa4.sin_port);
    else
        return ntohs(sa6.sin6_port);
}

void SocketAddress::setPort(ui16 port)
{
    sa4.sin_port = htons(port);
    sa6.sin6_port = htons(port);
}

void SocketAddress::setHostnameUTF8(const string &hostname)
{
    lock_guard<recursive_mutex> hostname_lock(hostname_mutex);
    this->hostname = hostname;
}

string SocketAddress::getHostnameUTF8() const
{
    unique_lock<recursive_mutex> hostname_lock(*const_cast<recursive_mutex*>(&hostname_mutex));
    string h = hostname;
    hostname_lock.unlock();

    return h;
}

ProtocolFamily SocketAddress::getIPProtocol() const
{
    return ip_protocol;
}

void SocketAddress::getRawIPv4Sockaddr(struct sockaddr_in *sai) const
{
    memcpy(sai, &sa4, sizeof(sa4));
}

void SocketAddress::getRawIPv6Sockaddr(struct sockaddr_in6 *sai) const
{
    memcpy(sai, &sa6, sizeof(sa6));
}

Socket::Socket()
{
    socket_desc = INVALID_SOCKET;
    listening_socket = false;
}

Socket::~Socket()
{
    unique_lock<recursive_mutex> lock(socket_mutex);
    close();
    lock.unlock();
}

Socket::Socket(SOCKET new_socket_desc, const SocketAddress &sa)
{
    socket_desc = new_socket_desc;
    listening_socket = false;
    socket_addr = SP<SocketAddress>(new SocketAddress(sa));
}

SOCKET Socket::getRawSocket()
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    return socket_desc;
}

bool Socket::createSocket(const SocketAddress &sa, bool also_bind)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    socket_desc = INVALID_SOCKET;
    if (sa.ip_protocol == IPv4)
        socket_desc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
    else
        socket_desc = socket(PF_INET6, SOCK_STREAM, IPPROTO_TCP);
    if (socket_desc == INVALID_SOCKET)
    {
        socket_error = string("socket() failure. ") + getErrorStringSocketsError(getSocketError());
        return false;
    }

    if (also_bind)
    {
        int result;
        const int flag = 1;
        if (setsockopt(socket_desc, SOL_SOCKET, SO_REUSEADDR, (const char*) &flag, sizeof(flag)))
        {
            socket_error = string("setsockopt() failure. ") + getErrorStringSocketsError(getSocketError());
            closesocket(socket_desc);
            socket_desc = INVALID_SOCKET;
            return false;
        }
        if (sa.ip_protocol == IPv4)
            result = ::bind(socket_desc, (struct sockaddr*) &sa.sa4, sizeof(sa.sa4));
        else
            result = ::bind(socket_desc, (struct sockaddr*) &sa.sa6, sizeof(sa.sa6));

        if (result)
        {
            socket_error = string("bind() failure. ") + getErrorStringSocketsError(getSocketError());
            closesocket(socket_desc);
            socket_desc = INVALID_SOCKET;
            return false;
        }
    }

    return true;
}

void Socket::connectAsynchronous_thread(SocketAddress sa, function1<void, bool> callback_function)
{
    int sdesc;
    sdesc = INVALID_SOCKET;
    if (sa.ip_protocol == IPv4)
        sdesc = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
    else
        sdesc = socket(PF_INET6, SOCK_STREAM, IPPROTO_TCP);
    if (sdesc == INVALID_SOCKET)
    {
        {
            lock_guard<recursive_mutex> lock(socket_mutex);
            socket_error = string("socket() failure. ") + getErrorStringSocketsError(getSocketError());
        }

        if (callback_function) callback_function(false);
        return;
    }

    int result = -1;
    if (sa.ip_protocol == IPv4)
        result = ::connect(sdesc, (struct sockaddr*) &sa.sa4, sizeof(sa.sa4));
    else
        result = ::connect(sdesc, (struct sockaddr*) &sa.sa6, sizeof(sa.sa6));

    try
    {
        this_thread::interruption_point();
        if (callback_function)
        {
            if (result != INVALID_SOCKET)
            {
                lock_guard<recursive_mutex> lock(socket_mutex);
                socket_desc = result;
                #ifndef _WIN32
                    int flags = fcntl(socket_desc, F_GETFL, 0);
                    fcntl(socket_desc, F_SETFL, flags | O_NONBLOCK);
                #else
                    unsigned long non_blocking_mode = 1;
                    ioctlsocket(socket_desc, FIONBIO, &non_blocking_mode);
                #endif

                socket_addr = SP<SocketAddress>(new SocketAddress(sa));
                listening_socket = false;
                callback_function(true);

                async_connect_thread = SP<boost::thread>();
                return;
            }
            else
                callback_function(false);
        }
    }
    catch (boost::thread_interrupted ti)
    {
        if (result != INVALID_SOCKET)
            closesocket(result);
        return;
    }

    lock_guard<recursive_mutex> lock(socket_mutex);
    async_connect_thread = SP<boost::thread>();
}

bool Socket::connectAsynchronous(const SocketAddress &sa, function1<void, bool> callback_function)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    if (async_connect_thread)
    {
        socket_error = "Asynchronous connecting already in progress.";
        return false;
    }
    if (socket_desc != INVALID_SOCKET)
    {
        socket_error = "Socket is active; cannot start asynchronous connecting.";
        return false;
    }

    async_connect_thread = SP<thread>(new thread(boost::bind(&Socket::connectAsynchronous_thread, this, sa, callback_function)));
    return true;
}

bool Socket::connect(const SocketAddress &sa)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    if (socket_desc != INVALID_SOCKET)
    {
        socket_error = "Socket is active; can't start connecting.";
        return false;
    }
    if (async_connect_thread)
    {
        socket_error = "Asynchronous connecting in progress; can't start connecting.";
        return false;
    }

    listening_socket = false;

    if (!createSocket(sa, false))
        return false;

    int result = -1;
    if (sa.ip_protocol == IPv4)
        result = ::connect(socket_desc, (struct sockaddr*) &sa.sa4, sizeof(sa.sa4));
    else
        result = ::connect(socket_desc, (struct sockaddr*) &sa.sa6, sizeof(sa.sa6));

    if (result == -1)
    {
        socket_error = string("connect() failure. ") + getErrorStringSocketsError(getSocketError());
        closesocket(socket_desc);
        socket_desc = INVALID_SOCKET;
        return false;
    }

#ifndef _WIN32
    int flags = fcntl(socket_desc, F_GETFL, 0);
    fcntl(socket_desc, F_SETFL, flags | O_NONBLOCK);
#else
    unsigned long non_blocking_mode = 1;
    ioctlsocket(socket_desc, FIONBIO, &non_blocking_mode);
#endif

    socket_addr = SP<SocketAddress>(new SocketAddress(sa));
    return true;
}

bool Socket::listen(const SocketAddress &sa)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    if (socket_desc != INVALID_SOCKET)
    {
        socket_error = "Socket is active; can't start listening.";
        return false;
    }
    if (async_connect_thread)
    {
        socket_error = "Asynchronous connecting in progress; can't start connecting.";
        return false;
    }

    listening_socket = false;

    if (!createSocket(sa, true))
        return false;

    int result = ::listen(socket_desc, 10);
    if (result)
    {
        socket_error = string("listen() failure. ") + getErrorStringSocketsError(getSocketError());
        closesocket(socket_desc);
        socket_desc = INVALID_SOCKET;
        return false;
    }

#ifndef _WIN32
    int flags = fcntl(socket_desc, F_GETFL, 0);
    fcntl(socket_desc, F_SETFL, flags | O_NONBLOCK);
#else
    unsigned long non_blocking_mode = 1;
    ioctlsocket(socket_desc, FIONBIO, &non_blocking_mode);
#endif

    socket_addr = SP<SocketAddress>(new SocketAddress(sa));
    listening_socket = true;
    return true;
}

void Socket::close()
{
    if (async_connect_thread)
    {
        async_connect_thread->interrupt();
        async_connect_thread->join();
        async_connect_thread = SP<boost::thread>();
    }

    lock_guard<recursive_mutex> lock(socket_mutex);

    if (socket_desc != INVALID_SOCKET)
        closesocket(socket_desc);
    socket_desc = INVALID_SOCKET;
    listening_socket = false;
    socket_addr = SP<SocketAddress>();
}

bool Socket::accept(Socket* accept_socket)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    if (socket_desc == INVALID_SOCKET) return false;
    if (accept_socket->socket_desc != INVALID_SOCKET) return false;
    if (!accept_socket) return false;
    if (!listening_socket) return false;

    SOCKET result;
    result = ::accept(socket_desc, NULL, NULL);
    if (result == INVALID_SOCKET)
    {
        if (getSocketError() == getEWOULDBLOCK()) return false;
        close();
        listening_socket = false;
        return false;
    }
    int new_socket_desc = result;

    sockaddr_max_t sa_struct;
    memset(&sa_struct, 0, sizeof(sockaddr_max_t));
    socklen_t sa_struct_len = sizeof(sa_struct);
    result = getpeername(new_socket_desc, (struct sockaddr*) &sa_struct, &sa_struct_len);
    if (result)
    {
        ::closesocket(new_socket_desc);
        return false;
    }

    SocketAddress sa;
    struct sockaddr* sa2 = (struct sockaddr*) &sa_struct;
    if (sa2->sa_family == PF_INET)
        sa = SocketAddress((struct sockaddr_in*) &sa_struct);
    else if (sa2->sa_family == PF_INET6)
        sa = SocketAddress((struct sockaddr_in6*) &sa_struct);
    else
    {
        /* Some unknown address family? hrrrr... */
        ::closesocket(new_socket_desc);
        return false;
    }

    accept_socket->socket_desc = new_socket_desc;
    accept_socket->socket_addr = SP<SocketAddress>(new SocketAddress(sa));
    accept_socket->listening_socket = false;

#ifndef _WIN32
    int flags = fcntl(new_socket_desc, F_GETFL, 0);
    fcntl(new_socket_desc, F_SETFL, flags | O_NONBLOCK);
#else
    unsigned long non_blocking_mode = 1;
    ioctlsocket(new_socket_desc, FIONBIO, &non_blocking_mode);
#endif
    return true;
}

SocketAddress Socket::getAddress()
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    if (!socket_addr) return SocketAddress();
    return *socket_addr.get();
}

static void set_async_socketaddress_hostname(string address_string, SP<SocketAddress> address)
{
    if (!address) return;

    /* For now, use regexes to throw away the port part
       of the address string. */
    Regex IPv4_regex("^([0-9.]+):(.+)$");
    if (IPv4_regex.execute(address_string))
        address_string = IPv4_regex.getMatch(1);
    else
    {
        Regex IPv6_regex("^\\[([0-9a-fA-F:]+)\\]:(.+)$");
        if (IPv6_regex.execute(address_string))
            address_string = IPv6_regex.getMatch(1);
        else
            return;
    }

    bool success = false;
    string hostname = reverseResolveUTF8(address_string, &success);
    if (!success) return;

    address->setHostnameUTF8(hostname);
}

void Socket::startAsynchronousReverseResolve()
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    string addr = socket_addr->getHumanReadablePlainUTF8();
    thread t(set_async_socketaddress_hostname, addr, socket_addr);
    t.detach();
}

bool Socket::active()
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    return (socket_desc != INVALID_SOCKET);
}

size_t Socket::send(const void* data, size_t datalen)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    if (socket_desc == INVALID_SOCKET) return 0;
    if (listening_socket) return 0;

    resetSocketError();
    int result = ::send(socket_desc, (const char*) data, datalen, 0);
    i32 er = getSocketError();
    if (result == 0 && er != getEWOULDBLOCK())
    {
        close();
        return 0;
    }
    if (result == -1)
    {
        if (getSocketError() == getEWOULDBLOCK()) return 0;
        close();
        return 0;
    }

    return result;
}

size_t Socket::recv(void* data, size_t datalen)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    if (socket_desc == INVALID_SOCKET) return 0;
    if (listening_socket) return 0;

    resetSocketError();
    int result = ::recv(socket_desc, (char*) data, datalen, 0);
    i32 er = getSocketError();
    if (result == 0)
    {
        if (er != getEWOULDBLOCK())
            close();
        return 0;
    }
    if (result == -1)
    {
        if (er == getEWOULDBLOCK()) return 0;
        close();
        return 0;
    }

    return result;
}

string Socket::recv(size_t datalen)
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    char* datachunk = new char[datalen];
    size_t result = recv((void*) datachunk, datalen);
    string result_str(datachunk, result);
    delete[] datachunk;
    return result_str;
}

string Socket::getError()
{
    lock_guard<recursive_mutex> lock(socket_mutex);
    return socket_error;
}

void trankesbel::initializeSockets()
{
    test_initialize_winsock();
}

void trankesbel::shutdownSockets()
{
    test_winsock_shutdown();
}

static void reverse_resolve_function(const SocketAddress &sa, function2<void, bool, string> signal_func)
{
    if (!signal_func) return;

    struct sockaddr_in sa4;
    struct sockaddr_in6 sa6;
    bool use_ipv4 = false;

    if (sa.getIPProtocol() == IPv4)
    {
        sa.getRawIPv4Sockaddr(&sa4);
        use_ipv4 = true;
    }
    else
        sa.getRawIPv6Sockaddr(&sa6);

    int max_wait = 20;

    char node_name[2001], service_name[2001];
    node_name[2000] = 0;
    service_name[2000] = 0;

    int result = 0;
    do
    {
        if (use_ipv4)
            result = getnameinfo((struct sockaddr*) &sa4, sizeof(sa4), node_name, 2000, service_name, 2000, NI_NAMEREQD);
        else
            result = getnameinfo((struct sockaddr*) &sa6, sizeof(sa6), node_name, 2000, service_name, 2000, NI_NAMEREQD);

        if (result == EAI_AGAIN)
            second_wait(1);
        else
            break;
    } while ( (max_wait--) > 0);

    if (!result)
        signal_func(true, string(node_name));
    else
        signal_func(false, getErrorStringEAI(result));
}

void trankesbel::reverseResolveUTF8(const SocketAddress &sa, function2<void, bool, string> func, bool block)
{
    thread reverse_resolve_thread(reverse_resolve_function, sa, func);
    if (block)
        reverse_resolve_thread.join();
    else
        reverse_resolve_thread.detach();
}

static void reverse_resolve_proxy(bool success, SocketAddress sa, string errormsg, function2<void, bool, string> func)
{
    if (!success)
        func(false, errormsg);

    reverse_resolve_function(sa, func);
}

void trankesbel::reverseResolveUTF8(string address, function2<void, bool, string> func, bool block)
{
    if (block)
    {
        bool success;
        string result_msg;

        SocketAddress sa = SocketAddress::resolveUTF8(address, "1", &success, &result_msg);
        if (!success)
        {
            func(false, result_msg);
            return;
        }

        reverseResolveUTF8(sa, func, true);
        return;
    }

    function3<void, bool, SocketAddress, string> callback;
    callback = boost::bind(reverse_resolve_proxy, _1, _2, _3, func);

    SocketAddress::resolve(address, "1", callback, false);
}

static void reverse_resolve_callback(bool success, string host_or_error, bool* success2, string* host_or_error2)
{
    (*success2) = success;
    (*host_or_error2) = host_or_error;
}

string trankesbel::reverseResolveUTF8(string address, bool* success2)
{
    bool success = false;
    string errormsg;

    function2<void, bool, string> callback;
    callback = boost::bind(reverse_resolve_callback, _1, _2, &success, &errormsg);

    reverseResolveUTF8(address, callback, true);

    if (success2) (*success2) = success;

    return errormsg;
}

std::string trankesbel::reverseResolveUTF8(const SocketAddress &sa, bool* success2)
{
    bool success = false;
    string errormsg;

    function2<void, bool, string> callback;
    callback = boost::bind(reverse_resolve_callback, _1, _2, &success, &errormsg);

    reverseResolveUTF8(sa, callback, true);

    if (success2) (*success2) = success;

    return errormsg;
}

ext: cc


text:
/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 */

#ifndef mapproxy_gdalsupport_types_hpp_included_
#define mapproxy_gdalsupport_types_hpp_included_

#include <boost/interprocess/smart_ptr/shared_ptr.hpp>
#include <boost/interprocess/smart_ptr/weak_ptr.hpp>
#include <boost/interprocess/allocators/allocator.hpp>
#include <boost/interprocess/smart_ptr/deleter.hpp>
#include <boost/interprocess/managed_external_buffer.hpp>
#include <boost/interprocess/indexes/flat_map_index.hpp>
#include <boost/interprocess/anonymous_shared_memory.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <boost/interprocess/containers/deque.hpp>
#include <boost/interprocess/containers/string.hpp>
#include <boost/interprocess/containers/vector.hpp>
#include <boost/interprocess/sync/interprocess_mutex.hpp>
#include <boost/interprocess/sync/interprocess_condition.hpp>
#include <boost/interprocess/sync/scoped_lock.hpp>

#include <boost/date_time/microsec_time_clock.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>

namespace bi = boost::interprocess;

typedef bi::basic_managed_external_buffer<
    char
    , bi::rbtree_best_fit<bi::mutex_family, void*>
    , bi::flat_map_index> ManagedBuffer;

typedef ManagedBuffer::segment_manager SegmentManager;

typedef bi::allocator<void, SegmentManager> Allocator;

typedef bi::basic_string<
    char, std::char_traits<char>
    , bi::allocator<char, SegmentManager>
    > String;

typedef bi::vector<String, bi::allocator<String, SegmentManager>> StringVector;

typedef bi::scoped_lock<bi::interprocess_mutex> Lock;

inline std::string asString(const String &str) {
    return { str.data(), str.size() };
}

inline boost::optional<std::string> asOptional(const String &str) {
    if (str.empty()) { return {}; }
    return std::string(str.data(), str.size());
}

inline boost::optional<geo::SrsDefinition>
asOptional(const String &str, geo::SrsDefinition::Type type)
{
    if (str.empty()) { return {}; }
    return geo::SrsDefinition(std::string(str.data(), str.size()), type);
}

struct ConstBlock {
    const char *data;
    std::size_t size;

    ConstBlock(const char *data = nullptr, std::size_t size = 0)
        : data(data), size(size)
    {}
};

#endif // mapproxy_gdalsupport_types_hpp_included_

max_stars_repo_name: maka-io/vts-mapproxy

max_issues_repo_name: maka-io/vts-mapproxy

max_forks_repo_name: maka-io/vts-mapproxy


text:
// Copyright 2020 Tier IV, Inc.
//
// 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 "surround_obstacle_checker/node.hpp"

#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_eigen/tf2_eigen.h>

#include <algorithm>
#include <functional>
#include <limits>
#include <memory>
#include <string>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options)
: Node("surround_obstacle_checker_node", node_options),
  tf_buffer_(this->get_clock()),
  tf_listener_(tf_buffer_),
  vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo())
{
  // Parameters
  use_pointcloud_ = this->declare_parameter("use_pointcloud", true);
  use_dynamic_object_ = this->declare_parameter("use_dynamic_object", true);
  surround_check_distance_ = this->declare_parameter("surround_check_distance", 2.0);
  surround_check_recover_distance_ =
    this->declare_parameter("surround_check_recover_distance", 2.5);
  state_clear_time_ = this->declare_parameter("state_clear_time", 2.0);
  stop_state_ego_speed_ = this->declare_parameter("stop_state_ego_speed", 0.1);
  debug_ptr_ = std::make_shared<SurroundObstacleCheckerDebugNode>(
    vehicle_info_.max_longitudinal_offset_m, this->get_clock(), *this);
  self_poly_ = createSelfPolygon();

  // Publishers
  path_pub_ =
    this->create_publisher<autoware_planning_msgs::msg::Trajectory>("~/output/trajectory", 1);
  stop_reason_diag_pub_ =
    this->create_publisher<diagnostic_msgs::msg::DiagnosticStatus>("~/output/no_start_reason", 1);

  // Subscriber
  path_sub_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>(
    "~/input/trajectory", 1,
    std::bind(&SurroundObstacleCheckerNode::pathCallback, this, std::placeholders::_1));
  pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
    "~/input/pointcloud", rclcpp::SensorDataQoS(),
    std::bind(&SurroundObstacleCheckerNode::pointCloudCallback, this, std::placeholders::_1));
  dynamic_object_sub_ =
    this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>(
      "~/input/objects", 1,
      std::bind(&SurroundObstacleCheckerNode::dynamicObjectCallback, this, std::placeholders::_1));
  current_velocity_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
    "~/input/twist", 1,
    std::bind(&SurroundObstacleCheckerNode::currentVelocityCallback, this, std::placeholders::_1));
}

void SurroundObstacleCheckerNode::pathCallback(
  const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg)
{
  if (use_pointcloud_ && !pointcloud_ptr_) {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for pointcloud info...");
    return;
  }

  if (use_dynamic_object_ && !object_ptr_) {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for dynamic object info...");
    return;
  }

  if (!current_velocity_ptr_) {
    RCLCPP_WARN_THROTTLE(
      this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for current velocity...");
    return;
  }

  // parameter description
  autoware_planning_msgs::msg::Trajectory output_msg = *input_msg;
  diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag;

  // get current pose in traj frame
  geometry_msgs::msg::Pose current_pose;
  if (!getPose(input_msg->header.frame_id, "base_link", current_pose)) {
    return;
  }

  // get closest idx
  const size_t closest_idx = getClosestIdx(*input_msg, current_pose);

  // get nearest object
  double min_dist_to_obj = std::numeric_limits<double>::max();
  geometry_msgs::msg::Point nearest_obj_point;
  getNearestObstacle(&min_dist_to_obj, &nearest_obj_point);

  // check current obstacle status (exist or not)
  const auto is_obstacle_found = isObstacleFound(min_dist_to_obj);

  // check current stop status (stop or not)
  const auto is_stopped = checkStop(input_msg->points.at(closest_idx));

  const auto is_stop_required = isStopRequired(is_obstacle_found, is_stopped);

  // insert stop velocity
  if (is_stop_required) {
    state_ = State::STOP;

    // do not start when there is a obstacle near the ego vehicle.
    RCLCPP_WARN_THROTTLE(
      get_logger(), *this->get_clock(), 500 /* ms */,
      "do not start because there is obstacle near the ego vehicle.");
    insertStopVelocity(closest_idx, &output_msg);

    // visualization for debug
    if (is_obstacle_found) {
      debug_ptr_->pushObstaclePoint(nearest_obj_point, PointType::NoStart);
    }
    debug_ptr_->pushPose(input_msg->points.at(closest_idx).pose, PoseType::NoStart);
    no_start_reason_diag = makeStopReasonDiag("obstacle", input_msg->points.at(closest_idx).pose);
  } else {
    state_ = State::PASS;
  }

  // publish trajectory and debug info
  path_pub_->publish(output_msg);
  stop_reason_diag_pub_->publish(no_start_reason_diag);
  debug_ptr_->publish();
}

void SurroundObstacleCheckerNode::pointCloudCallback(
  const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg)
{
  pointcloud_ptr_ = input_msg;
}

void SurroundObstacleCheckerNode::dynamicObjectCallback(
  const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg)
{
  object_ptr_ = input_msg;
}

void SurroundObstacleCheckerNode::currentVelocityCallback(
  const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg)
{
  current_velocity_ptr_ = input_msg;
}

void SurroundObstacleCheckerNode::insertStopVelocity(
  const size_t closest_idx, autoware_planning_msgs::msg::Trajectory * traj)
{
  // set zero velocity from closest idx to last idx
  for (size_t i = closest_idx; i < traj->points.size(); i++) {
    traj->points.at(i).twist.linear.x = 0.0;
  }
}

bool SurroundObstacleCheckerNode::getPose(
  const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose)
{
  try {
    // get transform from source to target
    geometry_msgs::msg::TransformStamped ros_src2tgt =
      tf_buffer_.lookupTransform(source, target, tf2::TimePointZero);
    // convert geometry_msgs::msg::Transform to geometry_msgs::msg::Pose
    tf2::Transform transform;
    tf2::fromMsg(ros_src2tgt.transform, transform);
    tf2::toMsg(transform, pose);
  } catch (tf2::TransformException & ex) {
    RCLCPP_WARN_STREAM_THROTTLE(
      get_logger(), *this->get_clock(), 500 /* ms */,
      "cannot get tf from " << source << " to " << target);
    return false;
  }
  return true;
}

bool SurroundObstacleCheckerNode::convertPose(
  const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target,
  const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose)
{
  tf2::Transform src2tgt;
  try {
    // get transform from source to target
    geometry_msgs::msg::TransformStamped ros_src2tgt =
      tf_buffer_.lookupTransform(source, target, time, tf2::durationFromSec(0.1));
    tf2::fromMsg(ros_src2tgt.transform, src2tgt);
  } catch (tf2::TransformException & ex) {
    RCLCPP_WARN_STREAM_THROTTLE(
      get_logger(), *this->get_clock(), 500 /* ms */,
      "cannot get tf from " << source << " to " << target);
    return false;
  }

  tf2::Transform src2obj;
  tf2::fromMsg(pose, src2obj);
  tf2::Transform tgt2obj = src2tgt.inverse() * src2obj;
  tf2::toMsg(tgt2obj, conv_pose);
  return true;
}

size_t SurroundObstacleCheckerNode::getClosestIdx(
  const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose current_pose)
{
  double min_dist = std::numeric_limits<double>::max();
  size_t min_dist_idx = 0;
  for (size_t i = 0; i < traj.points.size(); ++i) {
    const double x = traj.points.at(i).pose.position.x - current_pose.position.x;
    const double y = traj.points.at(i).pose.position.y - current_pose.position.y;
    const double dist = std::hypot(x, y);
    if (dist < min_dist) {
      min_dist_idx = i;
      min_dist = dist;
    }
  }
  return min_dist_idx;
}

void SurroundObstacleCheckerNode::getNearestObstacle(
  double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point)
{
  if (use_pointcloud_) {
    getNearestObstacleByPointCloud(min_dist_to_obj, nearest_obj_point);
  }

  if (use_dynamic_object_) {
    getNearestObstacleByDynamicObject(min_dist_to_obj, nearest_obj_point);
  }
}

void SurroundObstacleCheckerNode::getNearestObstacleByPointCloud(
  double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point)
{
  // wait to transform pointcloud
  geometry_msgs::msg::TransformStamped transform_stamped;
  try {
    transform_stamped = tf_buffer_.lookupTransform(
      "base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp,
      tf2::durationFromSec(0.5));
  } catch (tf2::TransformException & ex) {
    RCLCPP_WARN_STREAM_THROTTLE(
      get_logger(), *this->get_clock(), 500 /* ms */,
      "failed to get base_link to " << pointcloud_ptr_->header.frame_id << " transform.");
    return;
  }

  Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.transform).cast<float>();
  pcl::PointCloud<pcl::PointXYZ> pcl;
  pcl::fromROSMsg(*pointcloud_ptr_, pcl);
  pcl::transformPointCloud(pcl, pcl, isometry);
  for (const auto & p : pcl) {
    // create boost point
    Point2d boost_p(p.x, p.y);

    // calc distance
    const double dist_to_obj = boost::geometry::distance(self_poly_, boost_p);

    // get minimum distance to obj
    if (dist_to_obj < *min_dist_to_obj) {
      *min_dist_to_obj = dist_to_obj;
      nearest_obj_point->x = p.x;
      nearest_obj_point->y = p.y;
      nearest_obj_point->z = p.z;
    }
  }
}

void SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject(
  double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point)
{
  const auto obj_frame = object_ptr_->header.frame_id;
  const auto obj_time = object_ptr_->header.stamp;
  for (const auto & obj : object_ptr_->objects) {
    // change frame of obj_pose to base_link
    geometry_msgs::msg::Pose pose_baselink;
    if (!convertPose(
          obj.state.pose_covariance.pose, obj_frame, "base_link", obj_time, pose_baselink)) {
      return;
    }

    // create obj polygon
    Polygon2d obj_poly;
    if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
      obj_poly = createObjPolygon(pose_baselink, obj.shape.footprint);
    } else {
      obj_poly = createObjPolygon(pose_baselink, obj.shape.dimensions);
    }

    // calc distance
    const double dist_to_obj = boost::geometry::distance(self_poly_, obj_poly);

    // get minimum distance to obj
    if (dist_to_obj < *min_dist_to_obj) {
      *min_dist_to_obj = dist_to_obj;
      *nearest_obj_point = obj.state.pose_covariance.pose.position;
    }
  }
}

bool SurroundObstacleCheckerNode::isObstacleFound(const double min_dist_to_obj)
{
  const auto is_obstacle_inside_range = min_dist_to_obj < surround_check_distance_;
  const auto is_obstacle_outside_range = min_dist_to_obj > surround_check_recover_distance_;

  if (state_ == State::PASS) {
    return is_obstacle_inside_range;
  }

  if (state_ == State::STOP) {
    return !is_obstacle_outside_range;
  }

  throw std::runtime_error("invalid state");
}

bool SurroundObstacleCheckerNode::isStopRequired(
  const bool is_obstacle_found, const bool is_stopped)
{
  if (!is_stopped) {
    return false;
  }

  if (is_obstacle_found) {
    last_obstacle_found_time_ = std::make_shared<const rclcpp::Time>(this->now());
    return true;
  }

  if (state_ != State::STOP) {
    return false;
  }

  // Keep stop state
  if (last_obstacle_found_time_) {
    const auto elapsed_time = this->now() - *last_obstacle_found_time_;
    if (elapsed_time.seconds() <= state_clear_time_) {
      return true;
    }
  }

  last_obstacle_found_time_ = {};
  return false;
}

bool SurroundObstacleCheckerNode::checkStop(
  const autoware_planning_msgs::msg::TrajectoryPoint & closest_point)
{
  if (std::fabs(current_velocity_ptr_->twist.linear.x) > stop_state_ego_speed_) {
    // ego vehicle has high velocity now. not stop.
    return false;
  }

  const double closest_vel = closest_point.twist.linear.x;
  const double closest_acc = closest_point.accel.linear.x;

  if (closest_vel * closest_acc < 0) {
    // ego vehicle is about to stop (during deceleration). not stop.
    return false;
  }

  return true;
}

Polygon2d SurroundObstacleCheckerNode::createSelfPolygon()
{
  const double front = vehicle_info_.max_longitudinal_offset_m;
  const double rear = vehicle_info_.min_longitudinal_offset_m;
  const double left = vehicle_info_.max_lateral_offset_m;
  const double right = vehicle_info_.min_lateral_offset_m;

  Polygon2d poly;
  boost::geometry::exterior_ring(poly) = boost::assign::list_of<Point2d>(front, left)(front, right)(
    rear, right)(rear, left)(front, left);
  return poly;
}

Polygon2d SurroundObstacleCheckerNode::createObjPolygon(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size)
{
  // rename
  const double x = pose.position.x;
  const double y = pose.position.y;
  const double h = size.x;
  const double w = size.y;
  const double yaw = tf2::getYaw(pose.orientation);

  // create base polygon
  Polygon2d obj_poly;
  boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of<Point2d>(h / 2.0, w / 2.0)(
    -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0);

  // rotate polygon(yaw)
  boost::geometry::strategy::transform::rotate_transformer<boost::geometry::radian, double, 2, 2>
    rotate(-yaw);  // anti-clockwise -> :clockwise rotation
  Polygon2d rotate_obj_poly;
  boost::geometry::transform(obj_poly, rotate_obj_poly, rotate);

  // translate polygon(x, y)
  boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translate(x, y);
  Polygon2d translate_obj_poly;
  boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate);
  return translate_obj_poly;
}

Polygon2d SurroundObstacleCheckerNode::createObjPolygon(
  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint)
{
  // rename
  const double x = pose.position.x;
  const double y = pose.position.y;
  const double yaw = tf2::getYaw(pose.orientation);

  // create base polygon
  Polygon2d obj_poly;
  for (const auto point : footprint.points) {
    const Point2d point2d(point.x, point.y);
    obj_poly.outer().push_back(point2d);
  }

  // rotate polygon(yaw)
  boost::geometry::strategy::transform::rotate_transformer<boost::geometry::radian, double, 2, 2>
    rotate(-yaw);  // anti-clockwise -> :clockwise rotation
  Polygon2d rotate_obj_poly;
  boost::geometry::transform(obj_poly, rotate_obj_poly, rotate);

  // translate polygon(x, y)
  boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translate(x, y);
  Polygon2d translate_obj_poly;
  boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate);
  return translate_obj_poly;
}

diagnostic_msgs::msg::DiagnosticStatus SurroundObstacleCheckerNode::makeStopReasonDiag(
  const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose)
{
  diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag;
  diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv;
  no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
  no_start_reason_diag.name = "no_start_reason";
  no_start_reason_diag.message = no_start_reason;
  no_start_reason_diag_kv.key = "no_start_pose";
  no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose);
  no_start_reason_diag.values.push_back(no_start_reason_diag_kv);
  return no_start_reason_diag;
}

std::string SurroundObstacleCheckerNode::jsonDumpsPose(const geometry_msgs::msg::Pose & pose)
{
  const std::string json_dumps_pose =
    (boost::format(
       R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") %
     pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x %
     pose.orientation.y % pose.orientation.z)
      .str();
  return json_dumps_pose;
}

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(SurroundObstacleCheckerNode)


text:
#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/alignment/alignment.h>
#include <mmtbx/alignment/align.h>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace mmtbx { namespace alignment {
namespace {

  struct pairwise_global_wrappers
  {
    typedef pairwise_global w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("pairwise_global", no_init)
        .def(init<std::string const&, std::string const&>((
          arg("seq1"), arg("seq2"))))
        .def_readonly("result1", &w_t::result1)
        .def_readonly("result2", &w_t::result2)
      ;
    }
  };

  struct align_global_wrappers
  {
    typedef align w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>("align", no_init)
        .def(init<std::string const&,
                  std::string const&,
                  af::const_ref<float> const&,
                  std::string const&,
              float, float, std::string const&>((
          arg("seq_a"),
          arg("seq_b"),
          arg("masking"),
          arg("style")="global",
          arg("gap_opening_penalty")=1,
          arg("gap_extension_penalty")=1,
          arg("similarity_function")="identity")))
        .def(init<af::const_ref<std::string> const&,
                  af::const_ref<std::string> const&,
                  af::const_ref<float> const&,
                  std::string const&,
                  float, float, std::string const&>((
          arg("seq_a"),
          arg("seq_b"),
          arg("masking"),
          arg("style")="global",
          arg("gap_opening_penalty")=1,
          arg("gap_extension_penalty")=1,
          arg("similarity_function")="identity")))
        .add_property("M", make_getter(&w_t::M, rbv()))
        .add_property("D", make_getter(&w_t::D, rbv()))
        .add_property("I", make_getter(&w_t::I, rbv()))
        .add_property("E", make_getter(&w_t::E, rbv()))
      ;
    }
  };

  void init_module()
  {
    pairwise_global_wrappers::wrap();
    align_global_wrappers::wrap();
  }

} // namespace <anonymous>
}} // namespace mmtbx::alignment

BOOST_PYTHON_MODULE(mmtbx_alignment_ext)
{
  mmtbx::alignment::init_module();
}


text:
//
// $Id: SearchResultSetTest.cpp 204 2012-11-21 21:51:50Z chambm $
//
// 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.
//
// The Original Code is the Bumbershoot core library.
//
// The Initial Developer of the Original Code is Matt Chambers.
//
// Copyright 2011 Vanderbilt University
//
// Contributor(s):
//

#include "pwiz_src/pwiz/utility/misc/unit.hpp"
#include "pwiz_src/pwiz/utility/misc/Std.hpp"
#include "SearchResultSet.h"
#include <boost/foreach.hpp>

using namespace pwiz::util;
using namespace pwiz::proteome;
using namespace freicore;

struct TestSearchResult
{
    int score;
    int expectedRank;
    const char* sequence;
    bool nTerminusIsSpecific;
    bool cTerminusIsSpecific;
    bool isDecoy;
};

const TestSearchResult testSearchResults[] =
{
    {25, 1, "THEQUICKRWNFX", true, true, false},
    {15, 2, "UMPSVERTHELZYDG", true, true, false},
    {12, 3, "THEQUICQRWNFX", true, false, true},
    {10, 4, "SHESELLS", false, true, false},
    {10, 4, "SHESEIIS", false, true, false},
    {5,  5, "SEASHELLS", false, true, false},
    {5,  5, "SEASHELIS", false, false, true},
    {5,  5, "SEASHEIIS", true, true, true},
    {3,  6, "BYTHESEA", false, false, true},
    {1,  7, "SHRE", false, false, false}
};

const size_t testSearchResultsSize = sizeof(testSearchResults) / sizeof(TestSearchResult);

struct SearchResult : public BaseSearchResult
{
    SearchResult(const TestSearchResult& result)
    : BaseSearchResult(DigestedPeptide(Peptide(result.sequence, ModificationParsing_Auto),
                                       0, 0,
                                       result.nTerminusIsSpecific,
                                       result.cTerminusIsSpecific)),
      score(result.score),
      _isDecoy(result.isDecoy)
    {}

    int score;
    bool _isDecoy;

    SearchScoreList getScoreList() const
    {
        SearchScoreList result;
        result.push_back(SearchScore("score", (double) score));
        return result;
    }

    bool isDecoy() const {return _isDecoy;}

    bool operator< (const SearchResult& rhs) const
    {
        return score < rhs.score;
    }
};

typedef boost::shared_ptr<SearchResult> SearchResultPtr;

struct SearchResultReverseLessThan
{
    bool operator() (const SearchResult& lhs, const SearchResult& rhs) const
    {
        return lhs.score > rhs.score;
    }
};

struct SearchResultPtrReverseLessThan
{
    bool operator() (const SearchResultPtr& lhs, const SearchResultPtr& rhs) const
    {
        return lhs->score > rhs->score;
    }
};

void testSimpleSet()
{
    vector<SearchResultPtr> searchResultSortedList;
    SearchResultSet<SearchResult> searchResultSet;
    map<int, vector<SearchResultPtr> > expectedResultsByRank;

    for (size_t i=0; i < testSearchResultsSize; ++i)
    {
        const TestSearchResult& t = testSearchResults[i];
        SearchResultPtr result(new SearchResult(t));
        searchResultSortedList.push_back(result);
        searchResultSet.add(result);
        expectedResultsByRank[t.expectedRank].push_back(result);
    }

    sort(searchResultSortedList.begin(), searchResultSortedList.end(), SearchResultPtrReverseLessThan());

    {
        unit_assert(searchResultSet.size() == testSearchResultsSize);
        unit_assert(searchResultSet.current_ranks() == 7);

        unit_assert(searchResultSet.bestFullySpecificTarget()->score == 25);
        unit_assert(searchResultSet.bestSemiSpecificDecoy()->score == 12);
        unit_assert(searchResultSet.bestSemiSpecificTarget()->score == 10);
        unit_assert(searchResultSet.bestFullySpecificDecoy()->score == 5);
        unit_assert(searchResultSet.bestNonSpecificDecoy()->score == 5);
        unit_assert(searchResultSet.bestNonSpecificTarget()->score == 1);

        map<int, vector<SearchResultPtr> > actualResultsByRank = searchResultSet.byRank();

        for (int i=1; i <= testSearchResultsSize; ++i)
            BOOST_FOREACH(const SearchResultPtr& item, expectedResultsByRank[i])
            {
                vector<SearchResultPtr>& actualRank = actualResultsByRank[i];
                unit_assert(find(actualRank.begin(), actualRank.end(), item) != actualRank.end());
            }
    }

    // should have no effect
    searchResultSet.max_ranks(7);

    {
        unit_assert(searchResultSet.size() == testSearchResultsSize);
        unit_assert(searchResultSet.current_ranks() == 7);

        unit_assert(searchResultSet.bestFullySpecificTarget()->score == 25);
        unit_assert(searchResultSet.bestSemiSpecificDecoy()->score == 12);
        unit_assert(searchResultSet.bestSemiSpecificTarget()->score == 10);
        unit_assert(searchResultSet.bestFullySpecificDecoy()->score == 5);
        unit_assert(searchResultSet.bestNonSpecificDecoy()->score == 5);
        unit_assert(searchResultSet.bestNonSpecificTarget()->score == 1);

        map<int, vector<SearchResultPtr> > actualResultsByRank = searchResultSet.byRank();

        for (int i=1; i <= testSearchResultsSize; ++i)
            BOOST_FOREACH(const SearchResultPtr& item, expectedResultsByRank[i])
            {
                vector<SearchResultPtr>& actualRank = actualResultsByRank[i];
                unit_assert(find(actualRank.begin(), actualRank.end(), item) != actualRank.end());
            }
    }

    // cut ranks down to 5
    searchResultSet.max_ranks(5);

    {
        unit_assert(searchResultSet.size() == 8);
        unit_assert(searchResultSet.current_ranks() == 5);

        unit_assert(searchResultSet.bestFullySpecificTarget()->score == 25);
        unit_assert(searchResultSet.bestSemiSpecificDecoy()->score == 12);
        unit_assert(searchResultSet.bestSemiSpecificTarget()->score == 10);
        unit_assert(searchResultSet.bestFullySpecificDecoy()->score == 5);
        unit_assert(searchResultSet.bestNonSpecificDecoy()->score == 5);
        unit_assert(searchResultSet.bestNonSpecificTarget()->score == 1);

        map<int, vector<SearchResultPtr> > actualResultsByRank = searchResultSet.byRank();

        for (size_t i=1; i <= searchResultSet.current_ranks(); ++i)
            BOOST_FOREACH(const SearchResultPtr& item, expectedResultsByRank[i])
            {
                vector<SearchResultPtr>& actualRank = actualResultsByRank[i];
                unit_assert(find(actualRank.begin(), actualRank.end(), item) != actualRank.end());
            }
    }

    // cut ranks down to 3
    searchResultSet.max_ranks(3);

    {
        unit_assert(searchResultSet.size() == 3);
        unit_assert(searchResultSet.current_ranks() == 3);

        map<int, vector<SearchResultPtr> > actualResultsByRank = searchResultSet.byRank();

        for (size_t i=1; i <= searchResultSet.current_ranks(); ++i)
            BOOST_FOREACH(const SearchResultPtr& item, expectedResultsByRank[i])
            {
                vector<SearchResultPtr>& actualRank = actualResultsByRank[i];
                unit_assert(find(actualRank.begin(), actualRank.end(), item) != actualRank.end());
            }
    }

    const TestSearchResult bestTestResult = {100, 1, "BEST", false, false, false};
    SearchResultPtr bestResult(new SearchResult(bestTestResult));
    searchResultSet.add(bestResult);

    // bump the ranks up
    expectedResultsByRank.clear();
    expectedResultsByRank[1].push_back(bestResult);
    expectedResultsByRank[2].push_back(searchResultSortedList[0]);
    expectedResultsByRank[3].push_back(searchResultSortedList[1]);

    {
        // size should still be 3
        unit_assert(searchResultSet.size() == 3);
        unit_assert(searchResultSet.current_ranks() == 3);

        unit_assert(searchResultSet.bestNonSpecificTarget()->score == 100);
        unit_assert(searchResultSet.bestFullySpecificTarget()->score == 25);
        unit_assert(searchResultSet.bestSemiSpecificDecoy()->score == 12);
        unit_assert(searchResultSet.bestSemiSpecificTarget()->score == 10);
        unit_assert(searchResultSet.bestFullySpecificDecoy()->score == 5);
        unit_assert(searchResultSet.bestNonSpecificDecoy()->score == 5);

        map<int, vector<SearchResultPtr> > actualResultsByRank = searchResultSet.byRank();

        for (size_t i=1; i <= searchResultSet.current_ranks(); ++i)
            BOOST_FOREACH(const SearchResultPtr& item, expectedResultsByRank[i])
            {
                vector<SearchResultPtr>& actualRank = actualResultsByRank[i];
                unit_assert(find(actualRank.begin(), actualRank.end(), item) != actualRank.end());
            }
    }

    const TestSearchResult secondBestTestResult = {90, 2, "BETTER", false, false, true};
    SearchResultPtr secondBestResult(new SearchResult(secondBestTestResult));
    searchResultSet.add(secondBestResult);

    // bump the ranks up
    expectedResultsByRank.clear();
    expectedResultsByRank[1].push_back(bestResult);
    expectedResultsByRank[2].push_back(secondBestResult);
    expectedResultsByRank[3].push_back(searchResultSortedList[0]);

    {
        // size should still be 3
        unit_assert(searchResultSet.size() == 3);
        unit_assert(searchResultSet.current_ranks() == 3);

        unit_assert(searchResultSet.bestNonSpecificTarget()->score == 100);
        unit_assert(searchResultSet.bestNonSpecificDecoy()->score == 90);
        unit_assert(searchResultSet.bestFullySpecificTarget()->score == 25);
        unit_assert(searchResultSet.bestSemiSpecificDecoy()->score == 12);
        unit_assert(searchResultSet.bestSemiSpecificTarget()->score == 10);
        unit_assert(searchResultSet.bestFullySpecificDecoy()->score == 5);

        map<int, vector<SearchResultPtr> > actualResultsByRank = searchResultSet.byRank();

        for (size_t i=1; i <= searchResultSet.current_ranks(); ++i)
            BOOST_FOREACH(const SearchResultPtr& item, expectedResultsByRank[i])
            {
                vector<SearchResultPtr>& actualRank = actualResultsByRank[i];
                unit_assert(find(actualRank.begin(), actualRank.end(), item) != actualRank.end());
            }
    }

    // bump max_ranks up to 4
    searchResultSet.max_ranks(4);

    const TestSearchResult worstTestResult = {0, 2, "WRST", false, false, true};
    SearchResultPtr worstResult(new SearchResult(worstTestResult));
    searchResultSet.add(worstResult);

    expectedResultsByRank[4].push_back(worstResult);

    {
        // size should now be 4
        unit_assert(searchResultSet.size() == 4);
        unit_assert(searchResultSet.current_ranks() == 4);

        unit_assert(searchResultSet.bestNonSpecificTarget()->score == 100);
        unit_assert(searchResultSet.bestNonSpecificDecoy()->score == 90);
        unit_assert(searchResultSet.bestFullySpecificTarget()->score == 25);
        unit_assert(searchResultSet.bestSemiSpecificDecoy()->score == 12);
        unit_assert(searchResultSet.bestSemiSpecificTarget()->score == 10);
        unit_assert(searchResultSet.bestFullySpecificDecoy()->score == 5);

        map<int, vector<SearchResultPtr> > actualResultsByRank = searchResultSet.byRank();

        for (size_t i=1; i <= searchResultSet.current_ranks(); ++i)
            BOOST_FOREACH(const SearchResultPtr& item, expectedResultsByRank[i])
            {
                vector<SearchResultPtr>& actualRank = actualResultsByRank[i];
                unit_assert(find(actualRank.begin(), actualRank.end(), item) != actualRank.end());
            }
    }
}

/*void testSimpleReverseSet()
{
    topset<SearchResult, SearchResultReverseLessThan> searchResultSet;
    for (size_t i=0; i < testSearchResultsSize; ++i)
        searchResultSet.insert(searchResults[i]);

    {
        unit_assert(searchResultSet.size() == testSearchResultsSize);

        int actualRank = testSearchResultsSize + 1;
        BOOST_REVERSE_FOREACH(const SearchResult& item, searchResultSet)
            unit_assert(item.expectedRank == --actualRank);
    }

    // should have no effect
    searchResultSet.max_size(testSearchResultsSize);

    {
        unit_assert(searchResultSet.size() == testSearchResultsSize);

        int actualRank = testSearchResultsSize + 1;
        BOOST_REVERSE_FOREACH(const SearchResult& item, searchResultSet)
            unit_assert(item.expectedRank == --actualRank);
    }

    // cut size down to 2
    searchResultSet.max_size(3);

    {
        unit_assert(searchResultSet.size() == 3);

        int actualRank = testSearchResultsSize + 1;
        BOOST_REVERSE_FOREACH(const SearchResult& item, searchResultSet)
            unit_assert(item.expectedRank == --actualRank);
    }
}*/

const TestSearchResult testSearchResults2[] =
{
    {1, 2, "KVKPSFVCLRC(57)", false, false, true},
    {2, 1, "KVKPSFVC(57)LRCK", false, true, true},
    {2, 1, "VKPSFVCLRC(57)K", true, true, true}
};

const size_t testSearchResultsSize2 = sizeof(testSearchResults2) / sizeof(TestSearchResult);

void test2()
{
    SearchResultSet<SearchResult> searchResultSet(5);

    for (size_t i=0; i < testSearchResultsSize2; ++i)
    {
        const TestSearchResult& t = testSearchResults2[i];
        SearchResultPtr result(new SearchResult(t));
        searchResultSet.add(result);
    }

    unit_assert(searchResultSet.size() == 3);
    unit_assert(searchResultSet.current_ranks() == 2);
};

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

    try
    {
        testSimpleSet();
        //testSimpleReverseSet();
        test2();
        return 0;
    }
    catch (exception& e)
    {
        TEST_FAILED(e.what())
    }
    catch (...)
    {
        TEST_FAILED("Caught unknown exception.")
    }

    TEST_EPILOG
}

max_stars_repo_name: shze/myrimatch-deb

max_issues_repo_name: shze/myrimatch-deb

max_forks_repo_name: shze/myrimatch-deb