Commit 34ab923f authored by Valentin Platzgummer's avatar Valentin Platzgummer

rosbrige_msgs added, toJson returns rapidjson::Value

parent 47d6aa69
# 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
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)
/*
* 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;
}
This diff is collapsed.
#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;
}
}
#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
# 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
/*
* 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;
}
TEMPLATE = app
CONFIG += console c++11
CONFIG -= app_bundle
CONFIG -= qt
SOURCES += \
main.cpp
HEADERS += \
../rosbridgecpp/main.h
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment