RosBridgeClient.cpp

#include "RosBridgeClient.h"

#include <chrono>
#include <functional>
#include <thread>
#include <future>


struct Task{
    std::function<bool(void)> ready; // Condition under which command should be executed.
    std::function<void(void)> execute; // Command to execute.
    std::function<bool(void)> expired; // Returns true if the command is expired.
    std::function<void(void)> clear_up; // operation to perform if task expired.
    std::string name;
};

void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr<WsClient> client, const std::__cxx11::string &message)
{
#ifndef DEBUG
    (void)client_name;
#endif
    if (!client->on_open)
    {
#ifdef DEBUG
        client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
        client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif

#ifdef DEBUG
            std::cout << client_name << ": Opened connection" << std::endl;
            std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
            connection->send(message);
        };
    }

#ifdef DEBUG
    if (!client->on_message)
    {
        client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
            std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
        };
    }

    if (!client->on_close)
    {
        client->on_close = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, int status, const std::string & /*reason*/) {
            std::cout << client_name << ": Closed connection with status code " << status << std::endl;
        };
    }

    if (!client->on_error)
    {
        // See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings
        client->on_error = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, const SimpleWeb::error_code &ec) {
            std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl;
        };
    }

#endif
#ifdef DEBUG
    std::thread client_thread([client_name, client]() {
#else
    std::thread client_thread([client]() {
#endif
        client->start();

#ifdef DEBUG
        std::cout << client_name << ": Terminated" << std::endl;
#endif
        client->on_open = NULL;
        client->on_message = NULL;
        client->on_close = NULL;
        client->on_error = NULL;
#ifdef DEBUG
        std::cout << "start" << client_name << " client reference count:" << client.use_count() << std::endl;
        std::cout << client_name << " thread end" << std::endl;
#endif
    });

    client_thread.detach();
}

RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_path) :
    server_port_path(server_port_path)
  , isConnected(std::make_shared<std::atomic_bool>(false))
  , stopped(std::make_shared<std::atomic_bool>(false))
{
    // Start periodic thread to monitor connection status, advertised topics etc.
    periodic_thread = std::make_shared<std::thread> ([this] {
        std::list<Task> task_list;
        constexpr auto poll_interval = std::chrono::seconds(1);
        auto poll_time_point = std::chrono::high_resolution_clock::now();
        while ( !this->stopped->load() ) {
            // ====================================================================================
            // Add tasks.
            // ====================================================================================
            if ( std::chrono::high_resolution_clock::now() > poll_time_point) {
                poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
                std::cout << "Starting new poll." << std::endl;
                std::cout << "connected: " << this->isConnected->load() << std::endl;
                std::string reset_status_task_name = "reset_status_task";
                // Add status task if necessary.
                auto const it = std::find_if(task_list.begin(), task_list.end(),
                                             [&reset_status_task_name](const Task &t){
                    return t.name == reset_status_task_name;
                });
                if ( it == task_list.end() ){
                    std::cout << "Adding status_task" << std::endl;
                    // Check connection status.
                    auto status_set = std::make_shared<std::atomic_bool>(false);
                    auto status_client = std::make_shared<WsClient>(this->server_port_path);
                    status_client->on_open = [status_set, this](std::shared_ptr<WsClient::Connection>) {
                            std::cout << "status_client opened" << std::endl;
                            this->isConnected->store(true);
                            status_set->store(true);
                    };

                    std::thread status_thread([status_client]{
                        status_client->start();
                        status_client->on_open = NULL;
                        status_client->on_message = NULL;
                        status_client->on_close = NULL;
                        status_client->on_error = NULL;
                    });
                    status_thread.detach();

                    // Create task to reset isConnected after one second.
                    Task reset_task;
                    reset_task.name = reset_status_task_name;
                    // condition
                    auto now = std::chrono::high_resolution_clock::now();
                    const auto t_trigger = now +  std::chrono::seconds(1);
                    reset_task.ready = [t_trigger]{
                        return std::chrono::high_resolution_clock::now() > t_trigger;
                    };
                    // command
                    reset_task.execute = [status_client, status_set, this]{
                        status_client->stop();
                        this->isConnected->store(false);
                        status_set->store(true);
                    };
                    // expired
                    reset_task.expired = [status_set]{
                        return  status_set->load();
                    };
                    // clear up
                    reset_task.clear_up = [status_client, this]{
                        status_client->stop();
                    };
                    task_list.push_back(reset_task);
                }

                if ( this->isConnected->load() ){
                    // Add available topics task if neccessary.
                    std::string reset_topics_task_name = "reset_topics_task";
                    auto const topics_it = std::find_if(task_list.begin(), task_list.end(), [&reset_topics_task_name](const Task &t){
                        return t.name == reset_topics_task_name;
                    });
                    if ( topics_it == task_list.end() ){
                        // Call /rosapi/topics service.
                        std::cout << "Adding reset_topics_task" << std::endl;
                        auto topics_set = std::make_shared<std::atomic_bool>(false);
                        this->callService("/rosapi/topics", [topics_set, this](
                                          std::shared_ptr<WsClient::Connection> connection,
                                          std::shared_ptr<WsClient::InMessage> in_message){
                            std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
                            std::unique_lock<std::mutex> lk(this->mutex);
                            this->available_topics = in_message->string();
                            lk.unlock();
                            topics_set->store(true);
                            connection->send_close(1000);
                        });

                        // Create task to reset topics after one second.
                        Task reset_task;
                        reset_task.name = reset_topics_task_name;
                        // condition
                        auto now = std::chrono::high_resolution_clock::now();
                        auto t_trigger = now +  std::chrono::seconds(1);
                        reset_task.ready = [t_trigger]{
                            return std::chrono::high_resolution_clock::now() > t_trigger;
                        };
                        // command
                        reset_task.execute = [topics_set, this]{
                            std::unique_lock<std::mutex> lk(this->mutex);
                            this->available_topics.clear();
                            lk.unlock();
                            topics_set->store(true);
                        };
                        // expired
                        reset_task.expired = [topics_set]{
                            return  topics_set->load();
                        };
                        // clear up
                        reset_task.clear_up = [this]{
                            return;
                        };
                        task_list.push_back(reset_task);
                    }

                    // Add available services task if neccessary.
                    std::string reset_services_name = "reset_services_task";
                    auto const services_it = std::find_if(task_list.begin(), task_list.end(), [&reset_services_name](const Task &t){
                        return t.name == reset_services_name;
                    });
                    if ( services_it == task_list.end() ){
                        // Call /rosapi/services service.
                        std::cout << "Adding reset_services_task" << std::endl;
                        auto services_set = std::make_shared<std::atomic_bool>(false);
                        this->callService("/rosapi/services", [this, services_set](
                                          std::shared_ptr<WsClient::Connection> connection,
                                          std::shared_ptr<WsClient::InMessage> in_message){
                            std::cout << "/rosapi/services: " << in_message->string() << std::endl;
                            std::unique_lock<std::mutex> lk(this->mutex);
                            this->available_services = in_message->string();
                            lk.unlock();
                            services_set->store(true);
                            connection->send_close(1000);
                        });

                        // Create task to reset services after one second.
                        Task reset_task;
                        reset_task.name = reset_services_name;
                        // condition
                        auto now = std::chrono::high_resolution_clock::now();
                        auto t_trigger = now +  std::chrono::seconds(1);
                        reset_task.ready = [t_trigger]{
                            return std::chrono::high_resolution_clock::now() > t_trigger;
                        };
                        // command
                        reset_task.execute = [services_set, this]{
                            std::unique_lock<std::mutex> lk(this->mutex);
                            this->available_services.clear();
                            lk.unlock();
                            services_set->store(true);
                        };
                        // expired
                        reset_task.expired = [services_set]{
                            return  services_set->load();
                        };
                        // clear up
                        reset_task.clear_up = [this]{
                            return;
                        };
                        task_list.push_back(reset_task);
                    }
                } else {
                    std::lock_guard<std::mutex> lk(mutex);
                    available_topics.clear();
                    available_services.clear();
                }
            }

            // ====================================================================================
            // Process tasks.
            // ====================================================================================
            for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){
                std::cout << "processing task: " << task_it->name << std::endl;
                if ( !task_it->expired() ){
                    if ( task_it->ready() ){
                        std::cout << "executing task: " << task_it->name << std::endl;
                        task_it->execute();
                        task_it = task_list.erase(task_it);
                    } else {
                        std::cout << "noting to do for task: " << task_it->name << std::endl;
                        ++task_it;
                    }
                } else {
                    std::cout << "task expired: " << task_it->name << std::endl;
                    task_it->clear_up();
                    task_it = task_list.erase(task_it);
                }
                std::cout << std::endl;
            }

            std::cout << "task list size: " << task_list.size() << std::endl;

            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }

        // Clear up remaining tasks.
        for ( auto task_it = task_list.begin(); task_it != task_list.end(); ++task_it){
            task_it->clear_up();
        }
        task_list.clear();
        std::cout << "periodic thread end" << std::endl;
    });
}

RosbridgeWsClient::~RosbridgeWsClient()
{
    stopped->store(true);
    unsubscribeAll();
    unadvertiseAll();
    unadvertiseAllServices();
    for (auto& client : client_map)
    {
        removeClient(client.first);
    }
    periodic_thread->join();
}

bool RosbridgeWsClient::connected(){
    return isConnected->load();
}

void RosbridgeWsClient::addClient(const std::string &client_name)
{
    std::lock_guard<std::mutex> lk(mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it == client_map.end())
    {
        client_map[client_name] = std::make_shared<WsClient>(server_port_path);
    }
#ifdef DEBUG
    else
    {
        std::cerr << client_name << " has already been created" << std::endl;
    }
#endif
}

std::shared_ptr<WsClient> RosbridgeWsClient::getClient(const std::string &client_name)
{
    std::lock_guard<std::mutex> lk(mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
        return it->second;
    }
    return NULL;
}

void RosbridgeWsClient::stopClient(const std::string &client_name)
{
    std::lock_guard<std::mutex> lk(mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
        // Stop the client asynchronously in 100 ms.
        // This is to ensure, that all threads involving the client have been launched.
        std::shared_ptr<WsClient> client = it->second;
#ifdef DEBUG
        std::thread t([client, client_name](){
#else
        std::thread t([client](){
#endif
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            client->stop();
            // The next lines of code seem to cause a double free or corruption error, why?
//            client->on_open = NULL;
//            client->on_message = NULL;
//            client->on_close = NULL;
//            client->on_error = NULL;
#ifdef DEBUG
            std::cout << "removeClient thread: " << client_name << " reference count: " << client.use_count() << std::endl;
            std::cout << client_name << " has been removed" << std::endl;
#endif
        });
        t.detach();
    }
#ifdef DEBUG
    else
    {
        std::cerr << client_name << " has not been created" << std::endl;
    }
#endif
}

void RosbridgeWsClient::removeClient(const std::string &client_name)
{
    stopClient(client_name);
    {
        std::lock_guard<std::mutex> lk(mutex);
        std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
        if (it != client_map.end())
        {
            client_map.erase(it);
#ifdef DEBUG
            std::cout << "removeClient: " << client_name << " reference count: " << client.use_count() << std::endl;
#endif
        }
#ifdef DEBUG
        else
        {
            std::cerr << client_name << " has not been created" << std::endl;
        }
#endif
    }
}

std::string RosbridgeWsClient::getAdvertisedTopics(){
    std::lock_guard<std::mutex> lk(mutex);
    return available_topics;
}

std::string RosbridgeWsClient::getAdvertisedServices(){
    std::lock_guard<std::mutex> lk(mutex);
    return available_services;
}

bool RosbridgeWsClient::topicAvailable(const std::string &topic){
#ifdef DEBUG
    std::cout << "checking if topic " << topic << " is available" << std::endl;
#endif
    size_t pos;
    {
        std::lock_guard<std::mutex> lk(mutex);
        pos = available_topics.find(topic);
    }
    return pos != std::string::npos ? true : false;
}

void RosbridgeWsClient::advertise(const std::string &client_name, const std::string &topic, const std::string &type, const std::string &id)
{
    std::lock_guard<std::mutex> lk(mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
    if (it_client != client_map.end())
    {
        auto it_ser_top = std::find_if(service_topic_list.begin(),
                                       service_topic_list.end(),
                                       [topic](const EntryData &td){
            return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
        });
        if ( it_ser_top != service_topic_list.end()){
#ifdef DEBUG
            std::cerr << "topic: " << topic << " already advertised" << std::endl;
#endif
            return;
        }
        auto client = it_client->second;
        std::weak_ptr<WsClient> wpClient = client;
        service_topic_list.push_back(std::make_tuple(EntryType::AdvertisedTopic, topic, client_name, wpClient));

        std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
        if (id.compare("") != 0)
        {
            message += ", \"id\":\"" + id + "\"";
        }
        message = "{" + message + "}";

#ifdef DEBUG
        client->on_open = [this, topic, message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
        client->on_open = [this, topic, message](std::shared_ptr<WsClient::Connection> connection) {
#endif

#ifdef DEBUG
            std::cout << client_name << ": Opened connection" << std::endl;
            std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
            connection->send(message);
        };

        start(client_name, client, message);
    }
#ifdef DEBUG
    else
    {
        std::cerr << client_name << "has not been created" << std::endl;
    }
#endif
}

void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string &id){
    std::lock_guard<std::mutex> lk(mutex);
    auto it_ser_top = std::find_if(service_topic_list.begin(),
                                   service_topic_list.end(),
                                   [topic](const EntryData &td){
        return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
    });
    if ( it_ser_top == service_topic_list.end()){
#ifdef DEBUG
        std::cerr << "topic: " << topic << " not advertised" << std::endl;
#endif
        return;
    }

    std::string message = "\"op\":\"unadvertise\"";
    if (id.compare("") != 0)
    {
        message += ", \"id\":\"" + id + "\"";
    }
    message += ", \"topic\":\"" + topic + "\"";
    message = "{" + message + "}";

    std::string client_name = "topic_unadvertiser" + topic;
    auto client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
    client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
    client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif

#ifdef DEBUG
        std::cout << client_name << ": Opened connection" << std::endl;
        std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
        connection->send(message); // unadvertise
        connection->send_close(1000);
    };

    start(client_name, client, message);
    service_topic_list.erase(it_ser_top);
}

void RosbridgeWsClient::unadvertiseAll(){
    for (auto entry : service_topic_list){
        if ( std::get<integral(EntryEnum::EntryType)>(entry) == EntryType::AdvertisedTopic ){
            unadvertise(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
        }
    }
}

void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Document &msg, const std::string &id)
{
    std::lock_guard<std::mutex> lk(mutex);
    auto it_ser_top = std::find_if(service_topic_list.begin(),
                                   service_topic_list.end(),
                                   [&topic](const EntryData &td){
        return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
    });
    if ( it_ser_top == service_topic_list.end() ){
#ifdef DEBUG
        std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
#endif
        return;
    }
    rapidjson::StringBuffer strbuf;
    rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
    msg.Accept(writer);

    std::string client_name = "publish_client" + topic;
    std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString();

    if (id.compare("") != 0)
    {
        message += ", \"id\":\"" + id + "\"";
    }
    message = "{" + message + "}";

    std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
    publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
    publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
        std::cout << client_name << ": Opened connection" << std::endl;
        std::cout << client_name << ": Sending message." << std::endl;
        //std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
        connection->send(message);

        // TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
        connection->send_close(1000);
    };

    start(client_name, publish_client, message);
}

void RosbridgeWsClient::subscribe(const std::string &client_name, const std::string &topic, const InMessage &callback, const std::string &id, const std::string &type, int throttle_rate, int queue_length, int fragment_size, const std::string &compression)
{
    std::lock_guard<std::mutex> lk(mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
    if (it_client != client_map.end())
    {
        auto it_ser_top = std::find_if(service_topic_list.begin(),
                                       service_topic_list.end(),
                                       [topic](const EntryData &td){
            return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
        });
        if ( it_ser_top != service_topic_list.end()){
#ifdef DEBUG
            std::cerr << "topic: " << topic << " already advertised" << std::endl;
#endif
            return;
        }
        auto client = it_client->second;
        std::weak_ptr<WsClient> wpClient = client;
        service_topic_list.push_back(std::make_tuple(EntryType::SubscribedTopic, topic, client_name, wpClient));

        std::string message = "\"op\":\"subscribe\", \"topic\":\"" + topic + "\"";

        if (id.compare("") != 0)
        {
            message += ", \"id\":\"" + id + "\"";
        }
        if (type.compare("") != 0)
        {
            message += ", \"type\":\"" + type + "\"";
        }
        if (throttle_rate > -1)
        {
            message += ", \"throttle_rate\":" + std::to_string(throttle_rate);
        }
        if (queue_length > -1)
        {
            message += ", \"queue_length\":" + std::to_string(queue_length);
        }
        if (fragment_size > -1)
        {
            message += ", \"fragment_size\":" + std::to_string(fragment_size);
        }
        if (compression.compare("none") == 0 || compression.compare("png") == 0)
        {
            message += ", \"compression\":\"" + compression + "\"";
        }
        message = "{" + message + "}";

        client->on_message = callback;
        this->start(client_name, client, message); // subscribe to topic
    }
#ifdef DEBUG
    else
    {
        std::cerr << client_name << "has not been created" << std::endl;
    }
#endif
}

void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string &id){
    std::lock_guard<std::mutex> lk(mutex);
    auto it_ser_top = std::find_if(service_topic_list.begin(),
                                   service_topic_list.end(),
                                   [topic](const EntryData &td){
        return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
    });
    if ( it_ser_top == service_topic_list.end()){
#ifdef DEBUG
        std::cerr << "topic: " << topic << " not advertised" << std::endl;
#endif
        return;
    }

    std::string message = "\"op\":\"unsubscribe\"";
    if (id.compare("") != 0)
    {
        message += ", \"id\":\"" + id + "\"";
    }
    message += ", \"topic\":\"" + topic + "\"";
    message = "{" + message + "}";

    std::string client_name = "topic_unsubscriber" + topic;
    auto client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
    client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
    client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif

#ifdef DEBUG
        std::cout << client_name << ": Opened connection" << std::endl;
        std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
        connection->send(message); // unadvertise
        connection->send_close(1000);
    };

    start(client_name, client, message);
    service_topic_list.erase(it_ser_top);
}

void RosbridgeWsClient::unsubscribeAll(){
    for (auto entry : service_topic_list){
        if( std::get<integral(EntryEnum::EntryType)>(entry) == EntryType::SubscribedTopic ) {
            unsubscribe(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
        }
    }
}

void RosbridgeWsClient::advertiseService(const std::string &client_name, const std::string &service, const std::string &type, const InMessage &callback)
{
    std::lock_guard<std::mutex> lk(mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
    if (it_client != client_map.end())
    {
        auto it_ser_top = std::find_if(service_topic_list.begin(),
                                       service_topic_list.end(),
                                       [service](const EntryData &td){
            return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
        });
        if ( it_ser_top != service_topic_list.end()){
#ifdef DEBUG
            std::cerr << "service: " << service << " already advertised" << std::endl;
#endif
            return;
        }
        auto client = it_client->second;
        std::weak_ptr<WsClient> wpClient = client;
        service_topic_list.push_back(std::make_tuple(EntryType::AdvertisedService, service, client_name, wpClient));

        std::string message = "{\"op\":\"advertise_service\", \"service\":\"" + service + "\", \"type\":\"" + type + "\"}";

        it_client->second->on_message = callback;
        start(client_name, it_client->second, message);
    }
#ifdef DEBUG
    else
    {
        std::cerr << client_name << "has not been created" << std::endl;
    }
#endif
}

void RosbridgeWsClient::unadvertiseService(const std::string &service){
    std::lock_guard<std::mutex> lk(mutex);
    auto it_ser_top = std::find_if(service_topic_list.begin(),
                                   service_topic_list.end(),
                                   [service](const EntryData &td){
        return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
    });
    if ( it_ser_top == service_topic_list.end()){
#ifdef DEBUG
        std::cerr << "service: " << service << " not advertised" << std::endl;
#endif
        return;
    }

    std::string message = "\"op\":\"unadvertise_service\"";
    message += ", \"service\":\"" + service + "\"";
    message = "{" + message + "}";

    std::string client_name = "topic_unsubscriber" + service;
    auto client = std::make_shared<WsClient>(server_port_path);
#ifdef DEBUG
    client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
#else
    client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif

#ifdef DEBUG
        std::cout << client_name << ": Opened connection" << std::endl;
        std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
        connection->send(message); // unadvertise
        connection->send_close(1000);
    };

    start(client_name, client, message);
    service_topic_list.erase(it_ser_top);
}

void RosbridgeWsClient::unadvertiseAllServices(){
    for (auto entry : service_topic_list){
        if( std::get<integral(EntryEnum::EntryType)>(entry) == EntryType::AdvertisedService ) {
            unadvertiseService(std::get<integral(EntryEnum::ServiceTopicName)>(entry));
        }
    }
}

void RosbridgeWsClient::serviceResponse(const std::string &service, const std::string &id, bool result, const rapidjson::Document &values)
{
    std::string message = "\"op\":\"service_response\", \"service\":\"" + service + "\", \"result\":" + ((result)? "true" : "false");

    // Rosbridge somehow does not allow service_response to be sent without id and values
    // , so we cannot omit them even though the documentation says they are optional.
    message += ", \"id\":\"" + id + "\"";

    // Convert JSON document to string
    rapidjson::StringBuffer strbuf;
    rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
    values.Accept(writer);

    message += ", \"values\":" + std::string(strbuf.GetString());
    message = "{" + message + "}";

    std::string client_name = "service_response_client" + service;
    std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);

#ifdef DEBUG
    service_response_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
#else
    service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
#endif
#ifdef DEBUG
        std::cout << client_name << ": Opened connection" << std::endl;
        std::cout << client_name << ": Sending message: " << message << std::endl;
#endif
        connection->send(message);

        connection->send_close(1000);
    };

    start(client_name, service_response_client, message);
}

void RosbridgeWsClient::callService(const std::string &service, const InMessage &callback, const rapidjson::Document &args, const std::string &id, int fragment_size, const std::string &compression)
{
    std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\"";

    if (!args.IsNull())
    {
        rapidjson::StringBuffer strbuf;
        rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
        args.Accept(writer);

        message += ", \"args\":" + std::string(strbuf.GetString());
    }
    if (id.compare("") != 0)
    {
        message += ", \"id\":\"" + id + "\"";
    }
    if (fragment_size > -1)
    {
        message += ", \"fragment_size\":" + std::to_string(fragment_size);
    }
    if (compression.compare("none") == 0 || compression.compare("png") == 0)
    {
        message += ", \"compression\":\"" + compression + "\"";
    }
    message = "{" + message + "}";

    std::string client_name = "call_service_client" + service;
    std::shared_ptr<WsClient> call_service_client = std::make_shared<WsClient>(server_port_path);

    if (callback)
    {
        call_service_client->on_message = callback;
    }
    else
    {
        call_service_client->on_message = [client_name](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
#ifdef DEBUG
            std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
            std::cout << client_name << ": Sending close connection" << std::endl;
#else
            (void)in_message;
#endif
            connection->send_close(1000);
        };
    }

    start(client_name, call_service_client, message);
}

bool RosbridgeWsClient::serviceAvailable(const std::string &service)
{
#ifdef DEBUG
    std::cout << "checking if service " << service << " is available" << std::endl;
#endif
    size_t pos;
    {
        std::lock_guard<std::mutex> lk(mutex);
        pos = available_services.find(service);
    }
    return pos != std::string::npos ? true : false;
}

void RosbridgeWsClient::waitForService(const std::string &service)
{
    waitForService(service, stopped);
}

void RosbridgeWsClient::waitForService(const std::string &service, const std::shared_ptr<std::atomic_bool> stop)
{
#ifdef DEBUG
    auto s = std::chrono::high_resolution_clock::now();
    long counter = 0;
#endif
    const auto poll_interval = std::chrono::milliseconds(1000);
    auto end = std::chrono::high_resolution_clock::now() + poll_interval;
    while( !stop->load() )
    {
        if ( std::chrono::high_resolution_clock::now() > end ){
#ifdef DEBUG
            ++counter;
#endif
            if ( serviceAvailable(service) ){
                break;
            } else {
                end = std::chrono::high_resolution_clock::now() + poll_interval;
            }
        } else {
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
    };
#ifdef DEBUG
    auto e = std::chrono::high_resolution_clock::now();
    std::cout << "waitForService() " << service << " time: "
              << std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
              << " ms." << std::endl;
    std::cout << "waitForTopic() " << service << ": number of calls to topicAvailable: "
              << counter << std::endl;
#endif
}

void RosbridgeWsClient::waitForTopic(const std::string &topic)
{
    auto stop = std::make_shared<std::atomic_bool>(false);
    waitForTopic(topic, stop);
}

void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::shared_ptr<std::atomic_bool> stop)
{
#ifdef DEBUG
    auto s = std::chrono::high_resolution_clock::now();
    long counter = 0;
#endif
    const auto poll_interval = std::chrono::milliseconds(1000);
    auto end = std::chrono::high_resolution_clock::now() + poll_interval;
    while( !stop->load() )
    {
        if ( std::chrono::high_resolution_clock::now() > end ){
#ifdef DEBUG
            ++counter;
#endif
            if ( topicAvailable(topic) ){
                break;
            } else {
                end = std::chrono::high_resolution_clock::now() + poll_interval;
            }
        } else {
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
    };
#ifdef DEBUG
    auto e = std::chrono::high_resolution_clock::now();
    std::cout << "waitForTopic() " << topic << " time: "
              << std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
              << " ms." << std::endl;
    std::cout << "waitForTopic() " << topic << ": number of calls to topicAvailable: "
              << counter << std::endl;
#endif
}