diff --git a/test_rosbridge/rosbridge/main.cpp b/test_rosbridge/rosbridge/main.cpp index 444b8f61e61415429d73dc8d95b11e3194446066..fc8af308ee3ad893b89d12b8f43325c2a4833d0f 100644 --- a/test_rosbridge/rosbridge/main.cpp +++ b/test_rosbridge/rosbridge/main.cpp @@ -45,10 +45,19 @@ void publisherThread(RosbridgeWsClient& rbc, const std::future& futureObj) 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::Polygon polygon2(std::vector({rosbridge_msgs::Point32(1,1,3), + rosbridge_msgs::Point32(2,2,4), + rosbridge_msgs::Point32(3,3,5)})); rosbridge_msgs::PolygonStamped polygonStamped(header, polygon); - rapidjson::Document doc; - rapidjson::Value val = polygonStamped.toJson(doc.GetAllocator()); - val.Swap(doc); + rosbridge_msgs::PolygonStamped polygonStamped2(header, polygon2); + std::vector parray({polygonStamped, polygonStamped2}); + + std::vector 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); @@ -59,7 +68,7 @@ void publisherThread(RosbridgeWsClient& rbc, const std::future& futureObj) while (futureObj.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout) { - rbc.publish("/polygon_stamped_topic", doc); + rbc.publish("/polygon_array_topic", doc); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } @@ -76,7 +85,7 @@ int main() { // rbc.advertiseService("service_advertiser", "/zservice", "std_srvs/SetBool", advertiseServiceCallback); rbc.addClient("topic_advertiser"); - rbc.advertise("topic_advertiser", "/polygon_stamped_topic", "geometry_msgs/PolygonStamped"); + rbc.advertise("topic_advertiser", "/polygon_array_topic", "jsk_recognition_msgs/PolygonArray"); // rbc.addClient("topic_subscriber"); // rbc.subscribe("topic_subscriber", "/ztopic", subscriberCallback); diff --git a/test_rosbridge/rosbridge/snake.cpp b/test_rosbridge/rosbridge/snake.cpp index 1800daea1ef4bf907117701e3d4fc17843dd1f57..2da752baca9a78313f8c91cd8171c5b086925e07 100644 --- a/test_rosbridge/rosbridge/snake.cpp +++ b/test_rosbridge/rosbridge/snake.cpp @@ -13,15 +13,14 @@ namespace rosbridge_msgs { Point32::Point32(_Float32 x, _Float32 y, _Float32 z): x(x), y(y), z(z) {} - rapidjson::Value Point32::toJson(rapidjson::Document::AllocatorType &allocator) + bool Point32::toJson(rapidjson::Document &doc, 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); + doc.AddMember("x", rapidjson::Value().SetFloat(this->x), allocator); + doc.AddMember("y", rapidjson::Value().SetFloat(this->y), allocator); + doc.AddMember("z", rapidjson::Value().SetFloat(this->z), allocator); - return val; + return true; } @@ -32,14 +31,12 @@ namespace rosbridge_msgs { Time::Time(uint32_t secs, uint32_t nsecs): secs(secs), nsecs(nsecs) {} - rapidjson::Value Time::toJson(rapidjson::Document::AllocatorType &allocator) + bool Time::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) { - rapidjson::Value val(rapidjson::kObjectType); + doc.AddMember("secs", rapidjson::Value().SetUint(this->secs), allocator); + doc.AddMember("nsecs", rapidjson::Value().SetUint(this->nsecs), allocator); - val.AddMember("secs", rapidjson::Value().SetUint(this->secs), allocator); - val.AddMember("nsecs", rapidjson::Value().SetUint(this->nsecs), allocator); - - return val; + return true; } @@ -48,17 +45,18 @@ namespace rosbridge_msgs { //=================================================================== 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) {} + Header::Header(uint32_t seq, const Time &stamp, std::string frame_id): seq(seq), stamp(stamp), frame_id(frame_id) {} - rapidjson::Value Header::toJson(rapidjson::Document::AllocatorType &allocator) + bool Header::toJson(rapidjson::Document &doc, 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; + doc.AddMember("seq", rapidjson::Value().SetUint(this->seq), allocator); + rapidjson::Document stamp(rapidjson::kObjectType); + if (!this->stamp.toJson(stamp, allocator)) + return false; + doc.AddMember("stamp", stamp, allocator); + doc.AddMember("frame_id", rapidjson::Value().SetString(this->frame_id.data(), this->frame_id.length(), allocator), allocator); + + return true; } @@ -67,20 +65,22 @@ namespace rosbridge_msgs { //=================================================================== Polygon::Polygon(){} - Polygon::Polygon(std::vector points) : points(points) {} + Polygon::Polygon(const std::vector &points) : points(points) {} - rapidjson::Value Polygon::toJson(rapidjson::Document::AllocatorType &allocator) + bool Polygon::toJson(rapidjson::Document &doc, 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); + for(std::vector::iterator it = this->points.begin(); it != this->points.end(); ++it) { + rapidjson::Document point(rapidjson::kObjectType); + if (!it->toJson(point, allocator)) + return false; + points.PushBack(point, allocator); + } - val.AddMember("points", points, allocator); + doc.AddMember("points", points, allocator); - return val; + return true; } @@ -89,16 +89,20 @@ namespace rosbridge_msgs { //=================================================================== PolygonStamped::PolygonStamped() {} - PolygonStamped::PolygonStamped(Header header, Polygon polygon) : header(header), polygon(polygon){} + PolygonStamped::PolygonStamped(const Header &header, const Polygon &polygon) : header(header), polygon(polygon){} - rapidjson::Value PolygonStamped::toJson(rapidjson::Document::AllocatorType &allocator) + bool PolygonStamped::toJson(rapidjson::Document &doc, 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; + rapidjson::Document header(rapidjson::kObjectType); + if (!this->header.toJson(header, allocator)) + return false; + rapidjson::Document polygon(rapidjson::kObjectType); + if (!this->polygon.toJson(polygon, allocator)) + return false; + doc.AddMember("header", header, allocator); + doc.AddMember("polygon", polygon, allocator); + + return true; } @@ -108,37 +112,42 @@ namespace rosbridge_msgs { //=================================================================== PolygonArray::PolygonArray() {} - PolygonArray::PolygonArray(Header header, - std::vector polygons, - std::vector labels, - std::vector<_Float32> likelihood) + PolygonArray::PolygonArray(const Header &header, + const std::vector &polygons, + const std::vector &labels, + const std::vector<_Float32> &likelihood) : header(header), polygons(polygons), labels(labels), likelihood(likelihood) {} - rapidjson::Value PolygonArray::toJson(rapidjson::Document::AllocatorType &allocator) + bool PolygonArray::toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator) { - rapidjson::Value val(rapidjson::kObjectType); - - val.AddMember("header", this->header.toJson(allocator), allocator); + rapidjson::Document header(rapidjson::kObjectType); + if (!this->header.toJson(header, allocator)) + return false; + doc.AddMember("header", header, 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); + for(auto it = this->polygons.begin(); it != this->polygons.end(); ++it){ + rapidjson::Document polygon(rapidjson::kObjectType); + if (!it->toJson(polygon, allocator)) + return false; + polygons.PushBack(polygon, allocator); + } + doc.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); + doc.AddMember("labels", labels, allocator); - rapidjson::Value likelyhood(rapidjson::kArrayType); + rapidjson::Value likelihood(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); + likelihood.PushBack(rapidjson::Value().SetFloat(*it), allocator); + doc.AddMember("likelihood", likelihood, allocator); - return val; + return true; } } diff --git a/test_rosbridge/rosbridge/snake.h b/test_rosbridge/rosbridge/snake.h index 5112aec5d72dd2f27f664264bf387b3faa5b7d44..5b17f0617723635d014b5c07e4eb8eed0b948c7d 100644 --- a/test_rosbridge/rosbridge/snake.h +++ b/test_rosbridge/rosbridge/snake.h @@ -20,7 +20,7 @@ namespace rosbridge_msgs { Time(); Time(uint32_t secs, uint32_t nsecs); - rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator); uint32_t secs; uint32_t nsecs; @@ -31,9 +31,9 @@ namespace rosbridge_msgs { class Header{ public: Header(); - Header(uint32_t seq, Time stamp, std::string frame_id); + Header(uint32_t seq, const Time &stamp, std::string frame_id); - rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator); uint32_t seq; Time stamp; @@ -47,7 +47,7 @@ namespace rosbridge_msgs { Point32(); Point32(_Float32 x, _Float32 y, _Float32 z); - rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator); _Float32 x; _Float32 y; @@ -59,9 +59,9 @@ namespace rosbridge_msgs { class Polygon{ public: Polygon(); - Polygon(std::vector points); + Polygon(const std::vector &points); - rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator); std::vector points; }; @@ -71,9 +71,9 @@ namespace rosbridge_msgs { class PolygonStamped{ public: PolygonStamped(); - PolygonStamped(Header header, Polygon polygon); + PolygonStamped(const Header &header, const Polygon &polygon); - rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator); Header header; Polygon polygon; @@ -84,12 +84,12 @@ namespace rosbridge_msgs { class PolygonArray{ public: PolygonArray(); - PolygonArray(Header header, std::vector polygons, std::vector labels, std::vector<_Float32> likelihood); + PolygonArray(const Header &header, const std::vector &polygons, const std::vector &labels, const std::vector<_Float32> &likelihood); - rapidjson::Value toJson(rapidjson::Document::AllocatorType &allocator); + bool toJson(rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator); Header header; - std::vector polygons; + std::vector polygons; std::vector labels; std::vector<_Float32> likelihood; };