diff --git a/test_rosbridge/rosbridge/.gitignore b/test_rosbridge/rosbridge/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..fab7372d796ea95c80d02df6caa7eb2b411a7ac1 --- /dev/null +++ b/test_rosbridge/rosbridge/.gitignore @@ -0,0 +1,73 @@ +# This file is used to ignore files which are generated +# ---------------------------------------------------------------------------- + +*~ +*.autosave +*.a +*.core +*.moc +*.o +*.obj +*.orig +*.rej +*.so +*.so.* +*_pch.h.cpp +*_resource.rc +*.qm +.#* +*.*# +core +!core/ +tags +.DS_Store +.directory +*.debug +Makefile* +*.prl +*.app +moc_*.cpp +ui_*.h +qrc_*.cpp +Thumbs.db +*.res +*.rc +/.qmake.cache +/.qmake.stash + +# qtcreator generated files +*.pro.user* + +# xemacs temporary files +*.flc + +# Vim temporary files +.*.swp + +# Visual Studio generated files +*.ib_pdb_index +*.idb +*.ilk +*.pdb +*.sln +*.suo +*.vcproj +*vcproj.*.*.user +*.ncb +*.sdf +*.opensdf +*.vcxproj +*vcxproj.* + +# MinGW generated files +*.Debug +*.Release + +# Python byte code +*.pyc + +# Binaries +# -------- +*.dll +*.exe + diff --git a/test_rosbridge/rosbridge/CMakeLists.txt b/test_rosbridge/rosbridge/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..5e1560831e3ae830f63df2e242e6651f9b242072 --- /dev/null +++ b/test_rosbridge/rosbridge/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.5) + +project(rosbridge LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_executable(rosbridge main.cpp snake.cpp) +set(HEADER_FILES main.h snake.h) + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../rosbridgecpp) +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../rosbridgecpp rosbridgecpp) +target_link_libraries(${PROJECT_NAME} rosbridgecpp) diff --git a/test_rosbridge/rosbridge/main.cpp b/test_rosbridge/rosbridge/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..444b8f61e61415429d73dc8d95b11e3194446066 --- /dev/null +++ b/test_rosbridge/rosbridge/main.cpp @@ -0,0 +1,118 @@ +/* + * Created on: Apr 16, 2018 + * Author: Poom Pianpak + */ + +#include "main.h" +#include "snake.h" + +#include + +RosbridgeWsClient rbc("localhost:9090"); + +void advertiseServiceCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message) +{ + // message->string() is destructive, so we have to buffer it first + std::string messagebuf = in_message->string(); + std::cout << "advertiseServiceCallback(): Message Received: " << messagebuf << std::endl; + + rapidjson::Document document; + if (document.Parse(messagebuf.c_str()).HasParseError()) + { + std::cerr << "advertiseServiceCallback(): Error in parsing service request message: " << messagebuf << std::endl; + return; + } + + rapidjson::Document values(rapidjson::kObjectType); + rapidjson::Document::AllocatorType& allocator = values.GetAllocator(); + values.AddMember("success", document["args"]["data"].GetBool(), allocator); + values.AddMember("message", "from advertiseServiceCallback", allocator); + + rbc.serviceResponse(document["service"].GetString(), document["id"].GetString(), true, values); +} + +void callServiceCallback(std::shared_ptr connection, std::shared_ptr in_message) +{ + std::cout << "serviceResponseCallback(): Message Received: " << in_message->string() << std::endl; + connection->send_close(1000); +} + +void publisherThread(RosbridgeWsClient& rbc, const std::future& futureObj) +{ + rbc.addClient("topic_publisher"); + + rosbridge_msgs::Header header(0, rosbridge_msgs::Time(1,2), "/map"); + rosbridge_msgs::Polygon polygon(std::vector({rosbridge_msgs::Point32(1,1,1), + rosbridge_msgs::Point32(2,2,2), + rosbridge_msgs::Point32(3,3,3)})); + rosbridge_msgs::PolygonStamped polygonStamped(header, polygon); + rapidjson::Document doc; + rapidjson::Value val = polygonStamped.toJson(doc.GetAllocator()); + val.Swap(doc); + + // Write to stdout + rapidjson::OStreamWrapper out(std::cout); + // Write document... + rapidjson::Writer writer(out); + doc.Accept(writer); + std::cout << std::endl; + + while (futureObj.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout) + { + rbc.publish("/polygon_stamped_topic", doc); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + std::cout << "publisherThread stops()" << std::endl; +} + +void subscriberCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message) +{ + std::cout << "subscriberCallback(): Message Received: " << in_message->string() << std::endl; +} + +int main() { +// rbc.addClient("service_advertiser"); +// rbc.advertiseService("service_advertiser", "/zservice", "std_srvs/SetBool", advertiseServiceCallback); + + rbc.addClient("topic_advertiser"); + rbc.advertise("topic_advertiser", "/polygon_stamped_topic", "geometry_msgs/PolygonStamped"); + +// rbc.addClient("topic_subscriber"); +// rbc.subscribe("topic_subscriber", "/ztopic", subscriberCallback); + + // Test calling a service +// rapidjson::Document document(rapidjson::kObjectType); +// document.AddMember("data", true, document.GetAllocator()); +// rbc.callService("/zservice", callServiceCallback, document); + + // Test creating and stopping a publisher + { + // Create a std::promise object + std::promise exitSignal; + + // Fetch std::future object associated with promise + std::future futureObj = exitSignal.get_future(); + + // Starting Thread & move the future object in lambda function by reference + std::thread th(&publisherThread, std::ref(rbc), std::cref(futureObj)); + + // Wait for 10 sec + std::this_thread::sleep_for(std::chrono::seconds(10)); + + std::cout << "Asking publisherThread to Stop" << std::endl; + + // Set the value in promise + exitSignal.set_value(); + + // Wait for thread to join + th.join(); + } + + // Test removing clients +// rbc.removeClient("service_advertiser"); + rbc.removeClient("topic_advertiser"); +// rbc.removeClient("topic_subscriber"); + + std::cout << "Program terminated" << std::endl; +} diff --git a/test_rosbridge/rosbridge/main.h b/test_rosbridge/rosbridge/main.h new file mode 100644 index 0000000000000000000000000000000000000000..ee9a5bc6849847f9aea9fdb14ccb909f318e59ca --- /dev/null +++ b/test_rosbridge/rosbridge/main.h @@ -0,0 +1,367 @@ +#ifndef MAIN_H +#define MAIN_H +/* +* Created on: Apr 16, 2018 +* Author: Poom Pianpak +*/ + +#include "Simple-WebSocket-Server/client_ws.hpp" + +#include "rapidjson/include/rapidjson/document.h" +#include "rapidjson/include/rapidjson/writer.h" +#include "rapidjson/include/rapidjson/stringbuffer.h" +#include "rapidjson/include/rapidjson/ostreamwrapper.h" + +#include +#include +#include + +using WsClient = SimpleWeb::SocketClient; +using InMessage = std::function, std::shared_ptr)>; + +class RosbridgeWsClient +{ + std::string server_port_path; + std::unordered_map> client_map; + + void start(const std::string& client_name, std::shared_ptr client, const std::string& message) + { + if (!client->on_open) + { +#ifdef DEBUG + client->on_open = [client_name, message](std::shared_ptr connection) { +#else + client->on_open = [message](std::shared_ptr 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 /*connection*/, std::shared_ptr 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 /*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 /*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; + }); + + client_thread.detach(); + + // This is to make sure that the thread got fully launched before we do anything to it (e.g. remove) + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + +public: + RosbridgeWsClient(const std::string& server_port_path) + { + this->server_port_path = server_port_path; + } + + ~RosbridgeWsClient() + { + for (auto& client : client_map) + { + client.second->stop(); + client.second.reset(); + } + } + + void addClient(const std::string& client_name) + { + std::unordered_map>::iterator it = client_map.find(client_name); + if (it == client_map.end()) + { + client_map[client_name] = std::make_shared(server_port_path); + } +#ifdef DEBUG + else + { + std::cerr << client_name << " has already been created" << std::endl; + } +#endif + } + + std::shared_ptr getClient(const std::string& client_name) + { + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + return it->second; + } + return NULL; + } + + void stopClient(const std::string& client_name) + { + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + it->second->stop(); + it->second->on_open = NULL; + it->second->on_message = NULL; + it->second->on_close = NULL; + it->second->on_error = NULL; +#ifdef DEBUG + std::cout << client_name << " has been stopped" << std::endl; +#endif + } +#ifdef DEBUG + else + { + std::cerr << client_name << " has not been created" << std::endl; + } +#endif + } + + void removeClient(const std::string& client_name) + { + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + it->second->stop(); + it->second.reset(); + client_map.erase(it); +#ifdef DEBUG + std::cout << client_name << " has been removed" << std::endl; +#endif + } +#ifdef DEBUG + else + { + std::cerr << client_name << " has not been created" << std::endl; + } +#endif + } + + void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "") + { + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\""; + + if (id.compare("") != 0) + { + message += ", \"id\":\"" + id + "\""; + } + message = "{" + message + "}"; + + start(client_name, it->second, message); + } +#ifdef DEBUG + else + { + std::cerr << client_name << "has not been created" << std::endl; + } +#endif + } + + void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "") + { + rapidjson::StringBuffer strbuf; + rapidjson::Writer writer(strbuf); + msg.Accept(writer); + + std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString(); + + if (id.compare("") != 0) + { + message += ", \"id\":\"" + id + "\""; + } + message = "{" + message + "}"; + + std::shared_ptr publish_client = std::make_shared(server_port_path); + + publish_client->on_open = [message](std::shared_ptr connection) { +#ifdef DEBUG + std::cout << "publish_client: Opened connection" << std::endl; + std::cout << "publish_client: 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("publish_client", publish_client, message); + } + + void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "") + { + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + 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 + "}"; + + it->second->on_message = callback; + start(client_name, it->second, message); + } +#ifdef DEBUG + else + { + std::cerr << client_name << "has not been created" << std::endl; + } +#endif + } + + void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback) + { + std::unordered_map>::iterator it = client_map.find(client_name); + if (it != client_map.end()) + { + std::string message = "{\"op\":\"advertise_service\", \"service\":\"" + service + "\", \"type\":\"" + type + "\"}"; + + it->second->on_message = callback; + start(client_name, it->second, message); + } +#ifdef DEBUG + else + { + std::cerr << client_name << "has not been created" << std::endl; + } +#endif + } + + void 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 writer(strbuf); + values.Accept(writer); + + message += ", \"values\":" + std::string(strbuf.GetString()); + message = "{" + message + "}"; + + std::shared_ptr service_response_client = std::make_shared(server_port_path); + + service_response_client->on_open = [message](std::shared_ptr connection) { +#ifdef DEBUG + std::cout << "service_response_client: Opened connection" << std::endl; + std::cout << "service_response_client: Sending message: " << message << std::endl; +#endif + connection->send(message); + + connection->send_close(1000); + }; + + start("service_response_client", service_response_client, message); + } + + void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "") + { + std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\""; + + if (!args.IsNull()) + { + rapidjson::StringBuffer strbuf; + rapidjson::Writer 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::shared_ptr call_service_client = std::make_shared(server_port_path); + + if (callback) + { + call_service_client->on_message = callback; + } + else + { + call_service_client->on_message = [](std::shared_ptr connection, std::shared_ptr in_message) { +#ifdef DEBUG + std::cout << "call_service_client: Message received: " << in_message->string() << std::endl; + std::cout << "call_service_client: Sending close connection" << std::endl; +#endif + connection->send_close(1000); + }; + } + + start("call_service_client", call_service_client, message); + } +}; + + +#endif // MAIN_H diff --git a/test_rosbridge/rosbridge/snake.cpp b/test_rosbridge/rosbridge/snake.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1800daea1ef4bf907117701e3d4fc17843dd1f57 --- /dev/null +++ b/test_rosbridge/rosbridge/snake.cpp @@ -0,0 +1,144 @@ +#include "snake.h" + + + +namespace rosbridge_msgs { + + + + //=================================================================== + // Point32 + //=================================================================== + Point32::Point32(): x(0), y(0), z(0) {} + + Point32::Point32(_Float32 x, _Float32 y, _Float32 z): x(x), y(y), z(z) {} + + rapidjson::Value Point32::toJson(rapidjson::Document::AllocatorType &allocator) + { + rapidjson::Value val(rapidjson::kObjectType); + + val.AddMember("x", rapidjson::Value().SetFloat(this->x), allocator); + val.AddMember("y", rapidjson::Value().SetFloat(this->y), allocator); + val.AddMember("z", rapidjson::Value().SetFloat(this->z), allocator); + + return val; + } + + + //=================================================================== + // Time + //=================================================================== + Time::Time(): secs(0), nsecs(0) {} + + Time::Time(uint32_t secs, uint32_t nsecs): secs(secs), nsecs(nsecs) {} + + rapidjson::Value Time::toJson(rapidjson::Document::AllocatorType &allocator) + { + rapidjson::Value val(rapidjson::kObjectType); + + val.AddMember("secs", rapidjson::Value().SetUint(this->secs), allocator); + val.AddMember("nsecs", rapidjson::Value().SetUint(this->nsecs), allocator); + + return val; + } + + + //=================================================================== + // Header + //=================================================================== + Header::Header(): seq(0), frame_id("") {} + + Header::Header(uint32_t seq, Time stamp, std::string frame_id): seq(seq), stamp(stamp), frame_id(frame_id) {} + + rapidjson::Value Header::toJson(rapidjson::Document::AllocatorType &allocator) + { + rapidjson::Value val(rapidjson::kObjectType); + + val.AddMember("seq", rapidjson::Value().SetUint(this->seq), allocator); + val.AddMember("stamp", this->stamp.toJson(allocator), allocator); + val.AddMember("frame_id", rapidjson::Value().SetString(this->frame_id.data(), this->frame_id.length(), allocator), allocator); + + return val; + } + + + //=================================================================== + // Polygon + //=================================================================== + Polygon::Polygon(){} + + Polygon::Polygon(std::vector points) : points(points) {} + + rapidjson::Value Polygon::toJson(rapidjson::Document::AllocatorType &allocator) + { + rapidjson::Value val(rapidjson::kObjectType); + + rapidjson::Value points(rapidjson::kArrayType); + + for(std::vector::iterator it = this->points.begin(); it != this->points.end(); ++it) + points.PushBack(it->toJson(allocator), allocator); + + val.AddMember("points", points, allocator); + + return val; + } + + + //=================================================================== + // PolygonStamped + //=================================================================== + PolygonStamped::PolygonStamped() {} + + PolygonStamped::PolygonStamped(Header header, Polygon polygon) : header(header), polygon(polygon){} + + rapidjson::Value PolygonStamped::toJson(rapidjson::Document::AllocatorType &allocator) + { + rapidjson::Value val(rapidjson::kObjectType); + + val.AddMember("header", this->header.toJson(allocator), allocator); + val.AddMember("polygon", this->polygon.toJson(allocator), allocator); + + return val; + } + + + + //=================================================================== + // PolygonArray + //=================================================================== + PolygonArray::PolygonArray() {} + + PolygonArray::PolygonArray(Header header, + std::vector polygons, + std::vector labels, + std::vector<_Float32> likelihood) + : header(header), polygons(polygons), labels(labels), likelihood(likelihood) {} + + rapidjson::Value PolygonArray::toJson(rapidjson::Document::AllocatorType &allocator) + { + rapidjson::Value val(rapidjson::kObjectType); + + val.AddMember("header", this->header.toJson(allocator), allocator); + + rapidjson::Value polygons(rapidjson::kArrayType); + + for(auto it = this->polygons.begin(); it != this->polygons.end(); ++it) + polygons.PushBack(it->toJson(allocator), allocator); + val.AddMember("polygons", polygons, allocator); + + rapidjson::Value labels(rapidjson::kArrayType); + + for(auto it = this->labels.begin(); it != this->labels.end(); ++it) + labels.PushBack(rapidjson::Value().SetUint(*it), allocator); + val.AddMember("labels", labels, allocator); + + rapidjson::Value likelyhood(rapidjson::kArrayType); + + for(auto it = this->likelihood.begin(); it != this->likelihood.end(); ++it) + likelyhood.PushBack(rapidjson::Value().SetFloat(*it), allocator); + val.AddMember("likelyhood", likelyhood, allocator); + + return val; + } + +} diff --git a/test_rosbridge/rosbridge/snake.h b/test_rosbridge/rosbridge/snake.h new file mode 100644 index 0000000000000000000000000000000000000000..5112aec5d72dd2f27f664264bf387b3faa5b7d44 --- /dev/null +++ b/test_rosbridge/rosbridge/snake.h @@ -0,0 +1,98 @@ +#ifndef SNAKE_H +#define SNAKE_H + +#include + +#include "rapidjson/include/rapidjson/document.h" +#include "rapidjson/include/rapidjson/writer.h" +#include "rapidjson/include/rapidjson/stringbuffer.h" +#include "rapidjson/include/rapidjson/ostreamwrapper.h" + +using namespace std; + +// C++ implementation of ROS messages in json representation to communicate with rosbridge. +namespace rosbridge_msgs { + + // C++ representation of ros::Time with fromJson and toJson functions for rosbridge. + // fromJson not yet implemented. + class Time{ + public: + Time(); + Time(uint32_t secs, uint32_t nsecs); + + rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + + uint32_t secs; + uint32_t nsecs; + }; + + // C++ representation of std_msgs/Header with fromJson and toJson functions for rosbridge. + // fromJson not yet implemented. + class Header{ + public: + Header(); + Header(uint32_t seq, Time stamp, std::string frame_id); + + rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + + uint32_t seq; + Time stamp; + std::string frame_id; + }; + + // C++ representation of geometry_msgs/Point32 with fromJson and toJson functions for rosbridge. + // fromJson not yet implemented. + class Point32{ + public: + Point32(); + Point32(_Float32 x, _Float32 y, _Float32 z); + + rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + + _Float32 x; + _Float32 y; + _Float32 z; + }; + + // C++ representation of geometry_msgs/Polygon with fromJson and toJson functions for rosbridge. + // fromJson not yet implemented. + class Polygon{ + public: + Polygon(); + Polygon(std::vector points); + + rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + + std::vector points; + }; + + // C++ representation of geometry_msgs/PolygonStamped with fromJson and toJson functions for rosbridge. + // fromJson not yet implemented. + class PolygonStamped{ + public: + PolygonStamped(); + PolygonStamped(Header header, Polygon polygon); + + rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + + Header header; + Polygon polygon; + }; + + // C++ representation of jsk_recognition_msgs/PolygonArray with fromJson and toJson functions for rosbridge. + // fromJson not yet implemented. + class PolygonArray{ + public: + PolygonArray(); + PolygonArray(Header header, std::vector polygons, std::vector labels, std::vector<_Float32> likelihood); + + rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + + Header header; + std::vector polygons; + std::vector labels; + std::vector<_Float32> likelihood; + }; +} + +#endif // SNAKE_H diff --git a/test_rosbridge/rosbridgecpp b/test_rosbridge/rosbridgecpp new file mode 160000 index 0000000000000000000000000000000000000000..cb87f6966a1debe4af70a38ae56f988d99645d3b --- /dev/null +++ b/test_rosbridge/rosbridgecpp @@ -0,0 +1 @@ +Subproject commit cb87f6966a1debe4af70a38ae56f988d99645d3b diff --git a/test_rosbridge/test_rosbridge/.gitignore b/test_rosbridge/test_rosbridge/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..fab7372d796ea95c80d02df6caa7eb2b411a7ac1 --- /dev/null +++ b/test_rosbridge/test_rosbridge/.gitignore @@ -0,0 +1,73 @@ +# This file is used to ignore files which are generated +# ---------------------------------------------------------------------------- + +*~ +*.autosave +*.a +*.core +*.moc +*.o +*.obj +*.orig +*.rej +*.so +*.so.* +*_pch.h.cpp +*_resource.rc +*.qm +.#* +*.*# +core +!core/ +tags +.DS_Store +.directory +*.debug +Makefile* +*.prl +*.app +moc_*.cpp +ui_*.h +qrc_*.cpp +Thumbs.db +*.res +*.rc +/.qmake.cache +/.qmake.stash + +# qtcreator generated files +*.pro.user* + +# xemacs temporary files +*.flc + +# Vim temporary files +.*.swp + +# Visual Studio generated files +*.ib_pdb_index +*.idb +*.ilk +*.pdb +*.sln +*.suo +*.vcproj +*vcproj.*.*.user +*.ncb +*.sdf +*.opensdf +*.vcxproj +*vcxproj.* + +# MinGW generated files +*.Debug +*.Release + +# Python byte code +*.pyc + +# Binaries +# -------- +*.dll +*.exe + diff --git a/test_rosbridge/test_rosbridge/main.cpp b/test_rosbridge/test_rosbridge/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4f237d2e21d23411f06ccf00393dcd8cdf12f988 --- /dev/null +++ b/test_rosbridge/test_rosbridge/main.cpp @@ -0,0 +1,105 @@ +/* + * Created on: Apr 16, 2018 + * Author: Poom Pianpak + */ + +#include "rosbridge_ws_client.hpp" + +#include + +RosbridgeWsClient rbc("localhost:9090"); + +void advertiseServiceCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message) +{ + // message->string() is destructive, so we have to buffer it first + std::string messagebuf = in_message->string(); + std::cout << "advertiseServiceCallback(): Message Received: " << messagebuf << std::endl; + + rapidjson::Document document; + if (document.Parse(messagebuf.c_str()).HasParseError()) + { + std::cerr << "advertiseServiceCallback(): Error in parsing service request message: " << messagebuf << std::endl; + return; + } + + rapidjson::Document values(rapidjson::kObjectType); + rapidjson::Document::AllocatorType& allocator = values.GetAllocator(); + values.AddMember("success", document["args"]["data"].GetBool(), allocator); + values.AddMember("message", "from advertiseServiceCallback", allocator); + + rbc.serviceResponse(document["service"].GetString(), document["id"].GetString(), true, values); +} + +void callServiceCallback(std::shared_ptr connection, std::shared_ptr in_message) +{ + std::cout << "serviceResponseCallback(): Message Received: " << in_message->string() << std::endl; + connection->send_close(1000); +} + +void publisherThread(RosbridgeWsClient& rbc, const std::future& futureObj) +{ + rbc.addClient("topic_publisher"); + + rapidjson::Document d; + d.SetObject(); + d.AddMember("data", "Test message from /ztopic", d.GetAllocator()); + + while (futureObj.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout) + { + rbc.publish("/ztopic", d); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + std::cout << "publisherThread stops()" << std::endl; +} + +void subscriberCallback(std::shared_ptr /*connection*/, std::shared_ptr in_message) +{ + std::cout << "subscriberCallback(): Message Received: " << in_message->string() << std::endl; +} + +int main() { + rbc.addClient("service_advertiser"); + rbc.advertiseService("service_advertiser", "/zservice", "std_srvs/SetBool", advertiseServiceCallback); + + rbc.addClient("topic_advertiser"); + rbc.advertise("topic_advertiser", "/ztopic", "std_msgs/String"); + + rbc.addClient("topic_subscriber"); + rbc.subscribe("topic_subscriber", "/ztopic", subscriberCallback); + + // Test calling a service + rapidjson::Document document(rapidjson::kObjectType); + document.AddMember("data", true, document.GetAllocator()); + rbc.callService("/zservice", callServiceCallback, document); + + // Test creating and stopping a publisher + { + // Create a std::promise object + std::promise exitSignal; + + // Fetch std::future object associated with promise + std::future futureObj = exitSignal.get_future(); + + // Starting Thread & move the future object in lambda function by reference + std::thread th(&publisherThread, std::ref(rbc), std::cref(futureObj)); + + // Wait for 10 sec + std::this_thread::sleep_for(std::chrono::seconds(10)); + + std::cout << "Asking publisherThread to Stop" << std::endl; + + // Set the value in promise + exitSignal.set_value(); + + // Wait for thread to join + th.join(); + } + + // Test removing clients + rbc.removeClient("service_advertiser"); + rbc.removeClient("topic_advertiser"); + rbc.removeClient("topic_subscriber"); + + std::cout << "Program terminated" << std::endl; +} diff --git a/test_rosbridge/test_rosbridge/test_rosbridge.pro b/test_rosbridge/test_rosbridge/test_rosbridge.pro new file mode 100644 index 0000000000000000000000000000000000000000..27fcc05b0b02b1fce035d0fba268dd5d46b276df --- /dev/null +++ b/test_rosbridge/test_rosbridge/test_rosbridge.pro @@ -0,0 +1,10 @@ +TEMPLATE = app +CONFIG += console c++11 +CONFIG -= app_bundle +CONFIG -= qt + +SOURCES += \ + main.cpp + +HEADERS += \ + ../rosbridgecpp/main.h