Repository navigation
Expand file tree
/
Copy pathgroot2_protocol.h
More file actions
268 lines (225 loc) · 6.51 KB
/
Copy pathgroot2_protocol.h
File metadata and controls
268 lines (225 loc) · 6.51 KB
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
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
#pragma once
#include "behaviortree_cpp/basic_types.h"
#ifdef BTCPP_SYSTEM_JSON
#include <nlohmann/json.hpp>
#else
#include "behaviortree_cpp/contrib/json.hpp"
#endif
#include <array>
#include <condition_variable>
#include <cstdint>
#include <cstring>
#include <memory>
#include <mutex>
#include <random>
#include <stdexcept>
namespace BT::Monitor
{
/*
* All the messages exchange with the BT executor are multipart ZMQ request-replies.
*
* The first part of the request and the reply have fixed size and are described below.
* The request and reply must have the same value of the fields:
*
* - request_id
* - request_type
* - protocol_id
*/
enum RequestType : uint8_t
{
// Request the entire tree definition as XML
FULLTREE = 'T',
// Request the status of all the nodes
STATUS = 'S',
// retrieve the values in a set of blackboards
BLACKBOARD = 'B',
// Groot requests the insertion of a hook
HOOK_INSERT = 'I',
// Groot requests to remove a hook
HOOK_REMOVE = 'R',
// Notify Groot that we reached a breakpoint
BREAKPOINT_REACHED = 'N',
// Groot will unlock a breakpoint
BREAKPOINT_UNLOCK = 'U',
// receive the existing hooks in JSON format
HOOKS_DUMP = 'D',
// Remove all hooks. To be done before disconnecting Groot
REMOVE_ALL_HOOKS = 'A',
DISABLE_ALL_HOOKS = 'X',
// start/stop recordong
TOGGLE_RECORDING = 'r',
// get all transitions when recording
GET_TRANSITIONS = 't',
UNDEFINED = 0,
};
inline const char* ToString(const RequestType& type)
{
switch(type)
{
case RequestType::FULLTREE:
return "full_tree";
case RequestType::STATUS:
return "status";
case RequestType::BLACKBOARD:
return "blackboard";
case RequestType::HOOK_INSERT:
return "hook_insert";
case RequestType::HOOK_REMOVE:
return "hook_remove";
case RequestType::BREAKPOINT_REACHED:
return "breakpoint_reached";
case RequestType::BREAKPOINT_UNLOCK:
return "breakpoint_unlock";
case RequestType::REMOVE_ALL_HOOKS:
return "hooks_remove_all";
case RequestType::HOOKS_DUMP:
return "hooks_dump";
case RequestType::DISABLE_ALL_HOOKS:
return "disable_hooks";
case RequestType::TOGGLE_RECORDING:
return "toggle_recording";
case RequestType::GET_TRANSITIONS:
return "get_transitions";
case RequestType::UNDEFINED:
return "undefined";
}
return "undefined";
}
constexpr uint8_t kProtocolID = 2;
using TreeUniqueUUID = std::array<char, 16>;
struct RequestHeader
{
uint32_t unique_id = 0;
uint8_t protocol = kProtocolID;
RequestType type = RequestType::UNDEFINED;
static size_t size()
{
return sizeof(uint32_t) + sizeof(uint8_t) + sizeof(uint8_t);
}
RequestHeader() = default;
RequestHeader(RequestType type) : type(type)
{
// a random number for request_id will do
static std::random_device rd;
std::mt19937 mt(rd());
std::uniform_int_distribution<uint32_t> dist;
unique_id = dist(mt);
}
bool operator==(const RequestHeader& other) const
{
return type == other.type && unique_id == other.unique_id;
}
bool operator!=(const RequestHeader& other) const
{
return !(*this == other);
}
};
struct ReplyHeader
{
RequestHeader request;
TreeUniqueUUID tree_id = {};
static size_t size()
{
return RequestHeader::size() + 16;
}
};
template <typename T>
inline unsigned Serialize(char* buffer, unsigned offset, T value)
{
memcpy(buffer + offset, &value, sizeof(T));
return sizeof(T);
}
template <typename T>
inline unsigned Deserialize(const char* buffer, unsigned offset, T& value)
{
memcpy(&value, buffer + offset, sizeof(T));
return sizeof(T);
}
inline std::string SerializeHeader(const RequestHeader& header)
{
std::string buffer;
buffer.resize(6);
unsigned offset = 0;
offset += Serialize(buffer.data(), offset, header.protocol);
offset += Serialize(buffer.data(), offset, uint8_t(header.type));
offset += Serialize(buffer.data(), offset, header.unique_id);
return buffer;
}
inline std::string SerializeHeader(const ReplyHeader& header)
{
// copy the first part directly (6 bytes)
std::string buffer = SerializeHeader(header.request);
// add the following 16 bytes
unsigned const offset = 6;
buffer.resize(offset + 16);
Serialize(buffer.data(), offset, header.tree_id);
return buffer;
}
inline RequestHeader DeserializeRequestHeader(const std::string& buffer)
{
RequestHeader header;
unsigned offset = 0;
offset += Deserialize(buffer.data(), offset, header.protocol);
uint8_t type = 0;
offset += Deserialize(buffer.data(), offset, type);
header.type = static_cast<Monitor::RequestType>(type);
offset += Deserialize(buffer.data(), offset, header.unique_id);
return header;
}
inline ReplyHeader DeserializeReplyHeader(const std::string& buffer)
{
ReplyHeader header;
header.request = DeserializeRequestHeader(buffer);
unsigned const offset = 6;
Deserialize(buffer.data(), offset, header.tree_id);
return header;
}
struct Hook
{
using Ptr = std::shared_ptr<Hook>;
// used to enable/disable the breakpoint
bool enabled = true;
enum class Position
{
PRE = 0,
POST = 1
};
Position position = Position::PRE;
uint16_t node_uid = 0;
enum class Mode
{
BREAKPOINT = 0,
REPLACE = 1
};
// interactive breakpoints are unblocked using unlockBreakpoint()
Mode mode = Mode::BREAKPOINT;
// used by interactive breakpoints to wait for unlocking
std::condition_variable wakeup;
std::mutex mutex;
// set to true to unlock an interactive breakpoint
bool ready = false;
// once finished self-destroy
bool remove_when_done = false;
// result to be returned
NodeStatus desired_status = NodeStatus::SKIPPED;
};
inline void to_json(nlohmann::json& js, const Hook& bp)
{
js = nlohmann::json{ { "enabled", bp.enabled },
{ "uid", bp.node_uid },
{ "mode", int(bp.mode) },
{ "once", bp.remove_when_done },
{ "desired_status", toStr(bp.desired_status) },
{ "position", int(bp.position) } };
}
inline void from_json(const nlohmann::json& js, Hook& bp)
{
js.at("enabled").get_to(bp.enabled);
js.at("uid").get_to(bp.node_uid);
js.at("once").get_to(bp.remove_when_done);
bp.mode = static_cast<Hook::Mode>(js.at("mode").get<int>());
bp.position = static_cast<Hook::Position>(js.at("position").get<int>());
const std::string desired_value = js.at("desired_status").get<std::string>();
bp.desired_status = convertFromString<NodeStatus>(desired_value);
}
} // namespace BT::Monitor