YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Header.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <yarp/os/Header.h>
8
9#include <yarp/os/Bottle.h>
13#include <yarp/os/Time.h>
14
15#include <algorithm>
16#include <utility>
17
18
22
23namespace {
24YARP_OS_LOG_COMPONENT(HEADER, "yarp.os.Header");
25}
26
28{
29public:
32 std::string&& frameId) :
33 sequenceNumber(count),
34 timeStamp(time),
35 frameId(std::move(frameId))
36 {
37 }
38
39 Private() = default;
40
41 void clear()
42 {
43 sequenceNumber = Header::npos;
44 timeStamp = 0.0;
45 frameId.clear();
46 }
47
49 {
51 if (sequenceNumber == Header::npos) {
52 // npos is not used to store a valid header, just restart the
53 // counter
55 }
56 timeStamp = time;
57 }
58
61 std::string frameId;
62};
63
64
65Header::Header(Header::count_t count, yarp::conf::float64_t time, std::string frameId) :
66 mPriv(new Private(count, time, std::move(frameId)))
67{
68 yCAssert(HEADER, mPriv->frameId.find(' ') == std::string::npos);
69}
70
72 mPriv(new Private())
73{
74}
75
77 mPriv(new Private(*(rhs.mPriv)))
78{
79}
80
82 mPriv(std::exchange(rhs.mPriv, nullptr))
83{
84}
85
87{
88 delete mPriv;
89}
90
92{
93 if (&rhs != this) {
94 *mPriv = *(rhs.mPriv);
95 }
96 return *this;
97}
98
100{
101 if (&rhs != this) {
102 std::swap(mPriv, rhs.mPriv);
103 }
104 return *this;
105}
106
108{
109 return mPriv->sequenceNumber;
110}
111
113{
114 return mPriv->timeStamp;
115}
116
117std::string Header::frameId() const
118{
119 return mPriv->frameId;
120}
121
122bool Header::isValid() const
123{
124 return mPriv->sequenceNumber != npos;
125}
126
128{
129 if (connection.isTextMode()) {
130 std::string stampStr = connection.expectText();
133 int used;
134
135 int ret = std::sscanf(stampStr.c_str(), "%u %lg %n", &seqNum, &ts, &used);
136 if (ret != 2 && used > 0) {
137 mPriv->clear();
138 return false;
139 }
140 mPriv->sequenceNumber = seqNum;
141 mPriv->timeStamp = ts;
142 mPriv->frameId = stampStr.substr((size_t)used, stampStr.find(' ', used));
143 } else {
144 connection.convertTextMode();
145
146 // Read list length (must be 3 or 2 for compatibility with Stamp)
147 std::int32_t header = connection.expectInt32();
148 if (header != BOTTLE_TAG_LIST) {
149 mPriv->clear();
150 return false;
151 }
152 std::int32_t len = connection.expectInt32();
153 if (len != 2 && len != 3) {
154 mPriv->clear();
155 return false;
156 }
157
158 // Read sequence number
159 std::int32_t code = connection.expectInt32();
160 if (code != BOTTLE_TAG_INT32) {
161 mPriv->clear();
162 return false;
163 }
164 mPriv->sequenceNumber = static_cast<Header::count_t>(connection.expectInt32());
165
166 // Read timestamp
167 code = connection.expectInt32();
168 if (code != BOTTLE_TAG_FLOAT64) {
169 mPriv->clear();
170 return false;
171 }
172 mPriv->timeStamp = connection.expectFloat64();
173
174 // Read frameId (unless receiving a Stamp)
175 if (len == 3) {
176 code = connection.expectInt32();
177 if (code != BOTTLE_TAG_STRING) {
178 mPriv->clear();
179 return false;
180 }
181 mPriv->frameId = connection.expectString();
182 yCAssert(HEADER, mPriv->frameId.find(' ') == std::string::npos);
183 } else {
184 mPriv->frameId.clear();
185 }
186
187 if (connection.isError()) {
188 mPriv->clear();
189 return false;
190 }
191 }
192 return !connection.isError();
193}
194
195
196// In order to keep the Header class as much compatible as possible with
197// Stamp, if the frameId is empty, it is not serialized at all.
199{
200 if (connection.isTextMode()) {
201 static constexpr size_t buf_size = 512;
202 char buf[buf_size];
203 if (!mPriv->frameId.empty()) {
204 std::snprintf(buf, buf_size, "%d %.*g %s", mPriv->sequenceNumber, DBL_DIG, mPriv->timeStamp, mPriv->frameId.c_str());
205 } else {
206 std::snprintf(buf, buf_size, "%d %.*g", mPriv->sequenceNumber, DBL_DIG, mPriv->timeStamp);
207 }
208 connection.appendText(buf);
209 } else {
210 connection.appendInt32(BOTTLE_TAG_LIST); // nested structure
211 connection.appendInt32(mPriv->frameId.empty() ? 2 : 3); // with two or three elements
212 connection.appendInt32(BOTTLE_TAG_INT32);
213 connection.appendInt32(static_cast<std::int32_t>(mPriv->sequenceNumber));
214 connection.appendInt32(BOTTLE_TAG_FLOAT64);
215 connection.appendFloat64(mPriv->timeStamp);
216 if (!mPriv->frameId.empty()) {
217 connection.appendInt32(BOTTLE_TAG_STRING);
218 connection.appendString(mPriv->frameId);
219 }
220 connection.convertTextMode();
221 }
222 return !connection.isError();
223}
224
226{
227 mPriv->update(Time::now());;
228}
229
231{
232 mPriv->update(time);
233}
234
235void Header::setFrameId(std::string frameId)
236{
237 mPriv->frameId = std::move(frameId);
238}
#define BOTTLE_TAG_FLOAT64
Definition Bottle.h:25
#define BOTTLE_TAG_INT32
Definition Bottle.h:21
#define BOTTLE_TAG_STRING
Definition Bottle.h:26
#define BOTTLE_TAG_LIST
Definition Bottle.h:28
bool ret
Private(Header::count_t count, yarp::conf::float64_t time, std::string &&frameId)
Definition Header.cpp:30
yarp::conf::float64_t timeStamp
Definition Header.cpp:60
void update(yarp::conf::float64_t time)
Definition Header.cpp:48
Header::count_t sequenceNumber
Definition Header.cpp:59
A mini-server for performing network communication in the background.
An interface for reading from a network connection.
An interface for writing to a network connection.
An abstraction for a sequence number, time stamp and/or frame id.
Definition Header.h:24
std::uint32_t count_t
Definition Header.h:26
bool write(ConnectionWriter &connection) const override
Write this object to a network connection.
Definition Header.cpp:198
~Header() override
Destructor.
Definition Header.cpp:86
bool read(ConnectionReader &connection) override
Read this object from a network connection.
Definition Header.cpp:127
Header & operator=(const Header &rhs)
Copy assignment operator.
Definition Header.cpp:91
yarp::conf::float64_t timeStamp() const
Get the time stamp.
Definition Header.cpp:112
bool isValid() const
Check if this Header is valid.
Definition Header.cpp:122
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Header.cpp:225
static constexpr count_t npos
Maximum sequence number, after which an incrementing sequence should return to zero.
Definition Header.h:32
void setFrameId(std::string frameId)
Set the frame id for this header.
Definition Header.cpp:235
Header()
Construct an invalid Header.
Definition Header.cpp:71
std::string frameId() const
Get the frame id.
Definition Header.cpp:117
count_t count() const
Get the sequence number.
Definition Header.cpp:107
#define yCAssert(component, x)
#define YARP_OS_LOG_COMPONENT(name, name_string)
STL namespace.
double float64_t
Definition numeric.h:77
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121