@@ -171,48 +171,52 @@ struct PlanningControlModeParams
171
171
template <typename MessageType>
172
172
struct PublisherVariables
173
173
{
174
- double period_ns;
174
+ double period_ns{ 0.0 } ;
175
175
typename MessageType::SharedPtr empty_area_message;
176
176
typename MessageType::SharedPtr object_spawned_message;
177
177
typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
178
178
rclcpp::TimerBase::SharedPtr timer;
179
179
};
180
180
181
- template <typename T>
182
- struct PublisherVariablesMessageTypeExtractor ;
183
-
184
- template <typename MessageType>
185
- struct PublisherVariablesMessageTypeExtractor <PublisherVariables<MessageType>>
186
- {
187
- using Type = MessageType;
188
- };
189
-
190
- template <typename T, typename = std::void_t <>>
191
- struct has_header : std::false_type
181
+ struct PublisherVarAccessor
192
182
{
193
- };
183
+ template <typename T, typename = std::void_t <>>
184
+ struct has_header : std::false_type
185
+ {
186
+ };
194
187
195
- template <typename T>
196
- struct has_header <T, std::void_t <decltype(T::header)>> : std::true_type
197
- {
198
- };
188
+ template <typename T>
189
+ struct has_header <T, std::void_t <decltype(T::header)>> : std::true_type
190
+ {
191
+ };
192
+ template <typename T, typename = std::void_t <>>
193
+ struct has_stamp : std::false_type
194
+ {
195
+ };
199
196
200
- struct PublisherVarAccessor
201
- {
202
- // Method to set header time if available
203
197
template <typename T>
204
- void setHeaderTimeIfAvailable (T & publisherVar, const rclcpp::Time & time)
198
+ struct has_stamp <T, std:: void_t <decltype(T::stamp)>> : std::true_type
205
199
{
206
- if constexpr (has_header<typename T::MessageType>::value) {
207
- if (publisherVar.empty_area_message ) {
208
- publisherVar.empty_area_message ->header .stamp = time ;
209
- }
210
- if (publisherVar.object_spawned_message ) {
211
- publisherVar.object_spawned_message ->header .stamp = time ;
212
- }
200
+ };
201
+ template <typename MessageType>
202
+ void publishWithCurrentTime (
203
+ const PublisherVariables<MessageType> & publisherVar, const rclcpp::Time & current_time,
204
+ const bool is_object_spawned) const
205
+ {
206
+ std::unique_ptr<MessageType> msg_to_be_published = std::make_unique<MessageType>();
207
+
208
+ if (is_object_spawned) {
209
+ *msg_to_be_published = *publisherVar.object_spawned_message ;
210
+ } else {
211
+ *msg_to_be_published = *publisherVar.empty_area_message ;
212
+ }
213
+ if constexpr (has_header<MessageType>::value) {
214
+ msg_to_be_published->header .stamp = current_time;
215
+ } else if constexpr (has_stamp<MessageType>::value) {
216
+ msg_to_be_published->stamp = current_time;
213
217
}
218
+ publisherVar.publisher ->publish (std::move (msg_to_be_published));
214
219
}
215
-
216
220
// Set Period
217
221
template <typename T>
218
222
void setPeriod (T & publisherVar, double newPeriod)
@@ -227,62 +231,19 @@ struct PublisherVarAccessor
227
231
return publisherVar.period_ns ;
228
232
}
229
233
230
- // Set Empty Area Message
231
- template <typename T, typename Message>
232
- void setEmptyAreaMessage (T & publisherVar, typename Message::SharedPtr message)
233
- {
234
- publisherVar.empty_area_message = message;
235
- }
236
-
237
234
// Get Empty Area Message
238
235
template <typename T>
239
236
std::shared_ptr<void > getEmptyAreaMessage (const T & publisherVar) const
240
237
{
241
238
return std::static_pointer_cast<void >(publisherVar.empty_area_message );
242
239
}
243
240
244
- // Set Object Spawned Message
245
- template <typename T, typename Message>
246
- void setObjectSpawnedMessage (T & publisherVar, typename Message::SharedPtr message)
247
- {
248
- publisherVar.object_spawned_message = message;
249
- }
250
-
251
241
// Get Object Spawned Message
252
242
template <typename T>
253
243
std::shared_ptr<void > getObjectSpawnedMessage (const T & publisherVar) const
254
244
{
255
245
return std::static_pointer_cast<void >(publisherVar.object_spawned_message );
256
246
}
257
-
258
- // Set Publisher
259
- template <typename T, typename PublisherType>
260
- void setPublisher (
261
- T & publisherVar, typename rclcpp::Publisher<PublisherType>::SharedPtr publisher)
262
- {
263
- publisherVar.publisher = publisher;
264
- }
265
-
266
- // Get Publisher
267
- template <typename T>
268
- std::shared_ptr<void > getPublisher (const T & publisherVar) const
269
- {
270
- return std::static_pointer_cast<void >(publisherVar.publisher );
271
- }
272
-
273
- // Set Timer
274
- template <typename T>
275
- void setTimer (T & publisherVar, rclcpp::TimerBase::SharedPtr newTimer)
276
- {
277
- publisherVar.timer = newTimer;
278
- }
279
-
280
- // Get Timer
281
- template <typename T>
282
- std::shared_ptr<void > getTimer (const T & publisherVar) const
283
- {
284
- return std::static_pointer_cast<void >(publisherVar.timer );
285
- }
286
247
};
287
248
288
249
using PublisherVariablesVariant = std::variant<
@@ -410,6 +371,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
410
371
std::unordered_map<std::string, BufferVariant> message_buffers_;
411
372
std::optional<rclcpp::Time> last_test_environment_init_time_;
412
373
std::optional<rclcpp::Time> spawn_cmd_time_;
374
+ std::atomic<bool > is_object_spawned_{false };
413
375
bool is_test_environment_created_{false };
414
376
bool is_ego_initialized_{false };
415
377
bool is_route_set_{false };
0 commit comments