Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
/*
* 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)}));
Valentin Platzgummer
committed
rosbridge_msgs::Polygon polygon2(std::vector<rosbridge_msgs::Point32>({rosbridge_msgs::Point32(1,1,3),
rosbridge_msgs::Point32(2,2,4),
rosbridge_msgs::Point32(3,3,5)}));
rosbridge_msgs::PolygonStamped polygonStamped(header, polygon);
Valentin Platzgummer
committed
rosbridge_msgs::PolygonStamped polygonStamped2(header, polygon2);
std::vector<rosbridge_msgs::PolygonStamped> parray({polygonStamped, polygonStamped2});
std::vector<uint32_t> labels({1,2});
std::vector<_Float32> likelihood({1,2});
rosbridge_msgs::PolygonArray polygonArray(header, parray, labels, likelihood);
rapidjson::Document doc(rapidjson::kObjectType);
void(polygonArray.toJson(doc, doc.GetAllocator()));
// 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)
{
Valentin Platzgummer
committed
rbc.publish("/polygon_array_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");
Valentin Platzgummer
committed
rbc.advertise("topic_advertiser", "/polygon_array_topic", "jsk_recognition_msgs/PolygonArray");
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
// 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;
}