RosBridgeClient.cpp

#include "RosBridgeClient.h"

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

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)
{
    this->server_port_path = server_port_path;
}

RosbridgeWsClient::~RosbridgeWsClient()
{
    unsubscribeAll();
    unadvertiseAll();
    unadvertiseAllServices();
    for (auto& client : client_map)
    {
        removeClient(client.first);
    }
}

bool RosbridgeWsClient::connected(){

        auto p = std::make_shared<std::promise<void>>();
        auto future = p->get_future();

        auto status_client = std::make_shared<WsClient>(server_port_path);
        status_client->on_open = [p](std::shared_ptr<WsClient::Connection>) {
                p->set_value();
        };

        std::thread t([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;
        });

        auto status = future.wait_for(std::chrono::milliseconds(20));
        status_client->stop();
        t.join();
        bool connected = status == std::future_status::ready;
        //std::cout << "connected(): " << connected << std::endl;
        return connected;
    return true;
}

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(){
    auto pPromise(std::make_shared<std::promise<std::string>>());
    auto future = pPromise->get_future();
    this->callService("/rosapi/topics", [pPromise](
                      std::shared_ptr<WsClient::Connection> connection,
                      std::shared_ptr<WsClient::InMessage> in_message){
        pPromise->set_value(in_message->string());
        connection->send_close(1000);
    });
    auto status = future.wait_for(std::chrono::milliseconds(20)); // enought?
    return status == std::future_status::ready ? future.get() : std::string();
}

std::string RosbridgeWsClient::getAdvertisedServices(){
    auto pPromise(std::make_shared<std::promise<std::string>>());
    auto future = pPromise->get_future();
    // Call /rosapi/services to see if topic is already available.
    this->callService("/rosapi/services", [pPromise](
                      std::shared_ptr<WsClient::Connection> connection,
                      std::shared_ptr<WsClient::InMessage> in_message){
        pPromise->set_value(in_message->string());
        connection->send_close(1000);
    });
    auto status = future.wait_for(std::chrono::milliseconds(25)); // enought?
    return status == std::future_status::ready ? future.get() : std::string();
}

bool RosbridgeWsClient::topicAvailable(const std::string &topic){
#ifdef DEBUG
    std::cout << "checking if topic " << topic << " is available" << std::endl;
#endif
    std::string advertised_topics = this->getAdvertisedTopics();
    auto pos = advertised_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
    std::string advertised_services = this->getAdvertisedServices();
    auto pos = advertised_services.find(service);
    return pos != std::string::npos ? true : false;
}

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

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
    while( !stop->load() )
    {
#ifdef DEBUG
        ++counter;
#endif
        if ( serviceAvailable(service) ){
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Avoid excessive polling.
    };
#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
    while( !stop->load() )
    {
#ifdef DEBUG
        ++counter;
#endif
        if ( topicAvailable(topic) ){
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Avoid excessive polling.
    };
#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
}