rosbrige_msgs added, toJson returns rapidjson::Value

test_rosbridge/rosbridge/.gitignore

+# 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
+

test_rosbridge/rosbridge/CMakeLists.txt

+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)

test_rosbridge/rosbridge/main.cpp

+/*
+ *  Created on: Apr 16, 2018
+ *      Author: Poom Pianpak
+ */
+
+#include "main.h"
+#include "snake.h"
+
+#include <future>
+
+RosbridgeWsClient rbc("localhost:9090");
+
+void advertiseServiceCallback(std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> 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<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message)
+{
+  std::cout << "serviceResponseCallback(): Message Received: " << in_message->string() << std::endl;
+  connection->send_close(1000);
+}
+
+void publisherThread(RosbridgeWsClient& rbc, const std::future<void>& 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>({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<rapidjson::OStreamWrapper> 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<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> 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<void> exitSignal;
+
+    // Fetch std::future object associated with promise
+    std::future<void> 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;
+}

test_rosbridge/rosbridge/snake.cpp

+#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<Point32> points) : points(points) {}
+
+    rapidjson::Value Polygon::toJson(rapidjson::Document::AllocatorType &allocator)
+    {
+        rapidjson::Value val(rapidjson::kObjectType);
+
+        rapidjson::Value points(rapidjson::kArrayType);
+
+        for(std::vector<Point32>::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<Polygon> polygons,
+                               std::vector<uint32_t> 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;
+    }
+
+}

test_rosbridge/rosbridge/snake.h

+#ifndef SNAKE_H
+#define SNAKE_H
+
+#include <vector>
+
+#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<Point32> points);
+
+        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+
+        std::vector<Point32> 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<Polygon> polygons, std::vector<uint32_t> labels, std::vector<_Float32> likelihood);
+
+        rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator);
+
+        Header header;
+        std::vector<Polygon> polygons;
+        std::vector<uint32_t> labels;
+        std::vector<_Float32> likelihood;
+    };
+}
+
+#endif // SNAKE_H

rosbridgecpp @ cb87f696

+Subproject commit cb87f6966a1debe4af70a38ae56f988d99645d3b

test_rosbridge/test_rosbridge/.gitignore

+# 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
+

test_rosbridge/test_rosbridge/main.cpp

+/*
+ *  Created on: Apr 16, 2018
+ *      Author: Poom Pianpak
+ */
+
+#include "rosbridge_ws_client.hpp"
+
+#include <future>
+
+RosbridgeWsClient rbc("localhost:9090");
+
+void advertiseServiceCallback(std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> 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<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message)
+{
+  std::cout << "serviceResponseCallback(): Message Received: " << in_message->string() << std::endl;
+  connection->send_close(1000);
+}
+
+void publisherThread(RosbridgeWsClient& rbc, const std::future<void>& 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<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> 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<void> exitSignal;
+
+    // Fetch std::future object associated with promise
+    std::future<void> 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;
+}

test_rosbridge/test_rosbridge/test_rosbridge.pro

+TEMPLATE = app
+CONFIG += console c++11
+CONFIG -= app_bundle
+CONFIG -= qt
+
+SOURCES += \
+        main.cpp
+
+HEADERS += \
+    ../rosbridgecpp/main.h