perception_interfaces 1.0.0
Loading...
Searching...
No Matches
state_setters.h
Go to the documentation of this file.
1
30#pragma once
31
32#include <perception_msgs_utils/impl/state_index.h>
33#include <perception_msgs_utils/impl/checks.h>
34#include <perception_msgs_utils/impl/utils.h>
35
36
37namespace perception_msgs {
38
39namespace object_access {
40
48 inline void setX(ObjectState& state, const double val, const bool reset_covariance = true) {
49 sanityCheckContinuousState(state);
50 const int idx = indexX(state.model_id);
51 state.continuous_state[idx] = val;
52 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
53 }
54
63 template <typename T>
64 inline void setX(T& obj, const double val, const bool reset_covariance = true) {
65 setX(obj.state, val, reset_covariance);
66 }
67
75 inline void setY(ObjectState& state, const double val, const bool reset_covariance = true) {
76 sanityCheckContinuousState(state);
77 const int idx = indexY(state.model_id);
78 state.continuous_state[idx] = val;
79 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
80 }
81
90 template <typename T>
91 inline void setY(T& obj, const double val, const bool reset_covariance = true) {
92 setY(obj.state, val, reset_covariance);
93 }
94
102 inline void setZ(ObjectState& state, const double val, const bool reset_covariance = true) {
103 sanityCheckContinuousState(state);
104 const int idx = indexZ(state.model_id);
105 state.continuous_state[idx] = val;
106 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
107 }
108
117 template <typename T>
118 inline void setZ(T& obj, const double val, const bool reset_covariance = true) {
119 setZ(obj.state, val, reset_covariance);
120 }
121
129 inline void setVelLon(ObjectState& state, const double val, const bool reset_covariance = true) {
130 sanityCheckContinuousState(state);
131 const int idx = indexVelLon(state.model_id);
132 state.continuous_state[idx] = val;
133 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
134 }
135
144 template <typename T>
145 inline void setVelLon(T& obj, const double val, const bool reset_covariance = true) {
146 setVelLon(obj.state, val, reset_covariance);
147 }
148
156 inline void setVelLat(ObjectState& state, const double val, const bool reset_covariance = true) {
157 sanityCheckContinuousState(state);
158 const int idx = indexVelLat(state.model_id);
159 state.continuous_state[idx] = val;
160 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
161 }
162
171 template <typename T>
172 inline void setVelLat(T& obj, const double val, const bool reset_covariance = true) {
173 setVelLat(obj.state, val, reset_covariance);
174 }
175
183 inline void setAccLon(ObjectState& state, const double val, const bool reset_covariance = true) {
184 sanityCheckContinuousState(state);
185 const int idx = indexAccLon(state.model_id);
186 state.continuous_state[idx] = val;
187 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
188 }
189
198 template <typename T>
199 inline void setAccLon(T& obj, const double val, const bool reset_covariance = true) {
200 setAccLon(obj.state, val, reset_covariance);
201 }
202
210 inline void setAccLat(ObjectState& state, const double val, const bool reset_covariance = true) {
211 sanityCheckContinuousState(state);
212 const int idx = indexAccLat(state.model_id);
213 state.continuous_state[idx] = val;
214 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
215 }
216
225 template <typename T>
226 inline void setAccLat(T& obj, const double val, const bool reset_covariance = true) {
227 setAccLat(obj.state, val, reset_covariance);
228 }
229
237 inline void setRoll(ObjectState& state, const double val, const bool reset_covariance = true) {
238 sanityCheckContinuousState(state);
239 double capped_val = val;
240 while (capped_val > M_PI) capped_val -= 2 * M_PI;
241 while (capped_val < -M_PI) capped_val += 2 * M_PI;
242 const int idx = indexRoll(state.model_id);
243 state.continuous_state[idx] = capped_val;
244 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
245 }
246
255 template <typename T>
256 inline void setRoll(T& obj, const double val, const bool reset_covariance = true) {
257 setRoll(obj.state, val, reset_covariance);
258 }
259
267 inline void setRollRate(ObjectState& state, const double val, const bool reset_covariance = true) {
268 sanityCheckContinuousState(state);
269 const int idx = indexRollRate(state.model_id);
270 state.continuous_state[idx] = val;
271 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
272 }
273
282 template <typename T>
283 inline void setRollRate(T& obj, const double val, const bool reset_covariance = true) {
284 setRollRate(obj.state, val, reset_covariance);
285 }
286
294 inline void setPitch(ObjectState& state, const double val, const bool reset_covariance = true) {
295 sanityCheckContinuousState(state);
296 double capped_val = val;
297 while (capped_val > M_PI) capped_val -= 2 * M_PI;
298 while (capped_val < -M_PI) capped_val += 2 * M_PI;
299 const int idx = indexPitch(state.model_id);
300 state.continuous_state[idx] = capped_val;
301 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
302 }
303
312 template <typename T>
313 inline void setPitch(T& obj, const double val, const bool reset_covariance = true) {
314 setPitch(obj.state, val, reset_covariance);
315 }
316
324 inline void setPitchRate(ObjectState& state, const double val, const bool reset_covariance = true) {
325 sanityCheckContinuousState(state);
326 const int idx = indexPitchRate(state.model_id);
327 state.continuous_state[idx] = val;
328 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
329 }
330
339 template <typename T>
340 inline void setPitchRate(T& obj, const double val, const bool reset_covariance = true) {
341 setPitchRate(obj.state, val, reset_covariance);
342 }
343
351 inline void setYaw(ObjectState& state, const double val, const bool reset_covariance = true) {
352 sanityCheckContinuousState(state);
353 double capped_val = val;
354 while (capped_val > M_PI) capped_val -= 2 * M_PI;
355 while (capped_val < -M_PI) capped_val += 2 * M_PI;
356 const int idx = indexYaw(state.model_id);
357 state.continuous_state[idx] = capped_val;
358 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
359 }
360
369 template <typename T>
370 inline void setYaw(T& obj, const double val, const bool reset_covariance = true) {
371 setYaw(obj.state, val, reset_covariance);
372 }
373
381 inline void setYawRate(ObjectState& state, const double val, const bool reset_covariance = true) {
382 sanityCheckContinuousState(state);
383 const int idx = indexYawRate(state.model_id);
384 state.continuous_state[idx] = val;
385 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
386 }
387
396 template <typename T>
397 inline void setYawRate(T& obj, const double val, const bool reset_covariance = true) {
398 setYawRate(obj.state, val, reset_covariance);
399 }
400
408 inline void setSteeringAngleAck(ObjectState& state, const double val, const bool reset_covariance = true) {
409 sanityCheckContinuousState(state);
410 const int idx = indexSteeringAngleAck(state.model_id);
411 state.continuous_state[idx] = val;
412 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
413 }
414
423 template <typename T>
424 inline void setSteeringAngleAck(T& obj, const double val, const bool reset_covariance = true) {
425 setSteeringAngleAck(obj.state, val, reset_covariance);
426 }
427
435 inline void setSteeringAngleRateAck(ObjectState& state, const double val, const bool reset_covariance = true) {
436 sanityCheckContinuousState(state);
437 const int idx = indexSteeringAngleRateAck(state.model_id);
438 state.continuous_state[idx] = val;
439 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
440 }
441
450 template <typename T>
451 inline void setSteeringAngleRateAck(T& obj, const double val, const bool reset_covariance = true) {
452 setSteeringAngleRateAck(obj.state, val, reset_covariance);
453 }
454
462 inline void setSteeringAngleFront(ObjectState& state, const double val, const bool reset_covariance = true) {
463 sanityCheckContinuousState(state);
464 const int idx = indexSteeringAngleFront(state.model_id);
465 state.continuous_state[idx] = val;
466 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
467 }
468
477 template <typename T>
478 inline void setSteeringAngleFront(T& obj, const double val, const bool reset_covariance = true) {
479 setSteeringAngleFront(obj.state, val, reset_covariance);
480 }
481
489 inline void setSteeringAngleRear(ObjectState& state, const double val, const bool reset_covariance = true) {
490 sanityCheckContinuousState(state);
491 const int idx = indexSteeringAngleRear(state.model_id);
492 state.continuous_state[idx] = val;
493 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
494 }
495
504 template <typename T>
505 inline void setSteeringAngleRear(T& obj, const double val, const bool reset_covariance = true) {
506 setSteeringAngleRear(obj.state, val, reset_covariance);
507 }
508
516 inline void setWidth(ObjectState& state, const double val, const bool reset_covariance = true) {
517 sanityCheckContinuousState(state);
518 const int idx = indexWidth(state.model_id);
519 state.continuous_state[idx] = val;
520 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
521 }
522
531 template <typename T>
532 inline void setWidth(T& obj, const double val, const bool reset_covariance = true) {
533 setWidth(obj.state, val, reset_covariance);
534 }
535
543 inline void setLength(ObjectState& state, const double val, const bool reset_covariance = true) {
544 sanityCheckContinuousState(state);
545 const int idx = indexLength(state.model_id);
546 state.continuous_state[idx] = val;
547 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
548 }
549
558 template <typename T>
559 inline void setLength(T& obj, const double val, const bool reset_covariance = true) {
560 setLength(obj.state, val, reset_covariance);
561 }
562
570 inline void setHeight(ObjectState& state, const double val, const bool reset_covariance = true) {
571 sanityCheckContinuousState(state);
572 const int idx = indexHeight(state.model_id);
573 state.continuous_state[idx] = val;
574 if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
575 }
576
585 template <typename T>
586 inline void setHeight(T& obj, const double val, const bool reset_covariance = true) {
587 setHeight(obj.state, val, reset_covariance);
588 }
589
596 inline void setStandstill(ObjectState& state, const bool val) {
597 sanityCheckDiscreteState(state);
598 const int idx = indexStandstill(state.model_id);
599 state.discrete_state[idx] = val;
600 }
601
609 template <typename T>
610 inline void setStandstill(T& obj, const bool val) {
611 setStandstill(obj.state, val);
612 }
613
620 inline void setTrafficLightState(ObjectState& state, const uint8_t val) {
621 sanityCheckDiscreteState(state);
622 const int idx = indexTrafficLightState(state.model_id);
623 state.discrete_state[idx] = val;
624 }
625
634 template <typename T>
635 inline void setTrafficLightState(T& obj, const uint8_t val) {
636 setTrafficLightState(obj.state, val);
637 }
638
645 inline void setTrafficLightType(ObjectState& state, const uint8_t val) {
646 sanityCheckDiscreteState(state);
647 const int idx = indexTrafficLightType(state.model_id);
648 state.discrete_state[idx] = val;
649 }
650
659 template <typename T>
660 inline void setTrafficLightType(T& obj, const uint8_t val) {
661 setTrafficLightType(obj.state, val);
662 }
663
664} // namespace object_access
665
666} // namespace perception_msgs
checks.h
Object state sanity checks.
perception_msgs::object_access::sanityCheckContinuousState
void sanityCheckContinuousState(const ObjectState &state)
Perform sanity check on continuous state of given object state.
Definition checks.h:84
perception_msgs::object_access::sanityCheckDiscreteState
void sanityCheckDiscreteState(const ObjectState &state)
Perform sanity check on discrete state of given object state.
Definition checks.h:104
state_index.h
Object state vector indices based on state model.
perception_msgs::object_access::indexRollRate
int indexRollRate(const unsigned char &model_id)
Get the vector-index that stores the roll rate for a given model-id.
Definition state_index.h:217
perception_msgs::object_access::indexVelLon
int indexVelLon(const unsigned char &model_id)
Get the vector-index that stores the longitudinal velocity for a given model-id.
Definition state_index.h:114
perception_msgs::object_access::indexWidth
int indexWidth(const unsigned char &model_id)
Get the vector-index that stores the width for a given model-id.
Definition state_index.h:368
perception_msgs::object_access::indexRoll
int indexRoll(const unsigned char &model_id)
Get the vector-index that stores the roll for a given model-id.
Definition state_index.h:198
perception_msgs::object_access::indexLength
int indexLength(const unsigned char &model_id)
Get the vector-index that stores the length for a given model-id.
Definition state_index.h:385
perception_msgs::object_access::indexPitchRate
int indexPitchRate(const unsigned char &model_id)
Get the vector-index that stores the pitch rate for a given model-id.
Definition state_index.h:251
perception_msgs::object_access::indexYawRate
int indexYawRate(const unsigned char &model_id)
Get the vector-index that stores the yaw-rate for a given model-id.
Definition state_index.h:287
perception_msgs::object_access::indexHeight
int indexHeight(const unsigned char &model_id)
Get the vector-index that stores the height for a given model-id.
Definition state_index.h:402
perception_msgs::object_access::indexVelLat
int indexVelLat(const unsigned char &model_id)
Get the vector-index that stores the lateral velocity for a given model-id.
Definition state_index.h:135
perception_msgs::object_access::indexY
int indexY(const unsigned char &model_id)
Get the vector-index that stores the y-position for a given model-id.
Definition state_index.h:68
perception_msgs::object_access::indexSteeringAngleFront
int indexSteeringAngleFront(const unsigned char &model_id)
Get the vector-index that stores the front wheel angle for a given model-id.
Definition state_index.h:338
perception_msgs::object_access::indexSteeringAngleRear
int indexSteeringAngleRear(const unsigned char &model_id)
Get the vector-index that stores the rear wheel angle for a given model-id.
Definition state_index.h:353
perception_msgs::object_access::indexZ
int indexZ(const unsigned char &model_id)
Get the vector-index that stores the z-position for a given model-id.
Definition state_index.h:91
perception_msgs::object_access::indexAccLon
int indexAccLon(const unsigned char &model_id)
Get the vector-index that stores the longitudinal acceleration for a given model-id.
Definition state_index.h:156
perception_msgs::object_access::indexStandstill
int indexStandstill(const unsigned char &model_id)
Get the vector-index that stores the standstill indication for a given model-id.
Definition state_index.h:419
perception_msgs::object_access::indexTrafficLightType
int indexTrafficLightType(const unsigned char &model_id)
Get the vector-index that stores the traffic light type for a given model-id.
Definition state_index.h:453
perception_msgs::object_access::indexSteeringAngleRateAck
int indexSteeringAngleRateAck(const unsigned char &model_id)
Get the vector-index that stores the steering angle rate for a given model-id.
Definition state_index.h:323
perception_msgs::object_access::indexPitch
int indexPitch(const unsigned char &model_id)
Get the vector-index that stores the pitch for a given model-id.
Definition state_index.h:232
perception_msgs::object_access::indexAccLat
int indexAccLat(const unsigned char &model_id)
Get the vector-index that stores the lateral acceleration for a given model-id.
Definition state_index.h:177
perception_msgs::object_access::indexSteeringAngleAck
int indexSteeringAngleAck(const unsigned char &model_id)
Get the vector-index that stores the ackermann steering angle for a given model-id.
Definition state_index.h:308
perception_msgs::object_access::indexX
int indexX(const unsigned char &model_id)
Get the vector-index that stores the x-position for a given model-id.
Definition state_index.h:45
perception_msgs::object_access::indexTrafficLightState
int indexTrafficLightState(const unsigned char &model_id)
Get the vector-index that stores the traffic light state for a given model-id.
Definition state_index.h:437
perception_msgs::object_access::indexYaw
int indexYaw(const unsigned char &model_id)
Get the vector-index that stores the yaw for a given model-id.
Definition state_index.h:266
perception_msgs::object_access::setYawRate
void setYawRate(ObjectState &state, const double val, const bool reset_covariance=true)
Set the yaw-rate for a given object state.
Definition state_setters.h:381
perception_msgs::object_access::setSteeringAngleRear
void setSteeringAngleRear(ObjectState &state, const double val, const bool reset_covariance=true)
Set the rear wheel angle for a given object state.
Definition state_setters.h:489
perception_msgs::object_access::setHeight
void setHeight(ObjectState &state, const double val, const bool reset_covariance=true)
Set the height for a given object state.
Definition state_setters.h:570
perception_msgs::object_access::setAccLat
void setAccLat(ObjectState &state, const double val, const bool reset_covariance=true)
Set the lateral acceleration for a given object state.
Definition state_setters.h:210
perception_msgs::object_access::setRollRate
void setRollRate(ObjectState &state, const double val, const bool reset_covariance=true)
Set the roll-rate for a given object state.
Definition state_setters.h:267
perception_msgs::object_access::setSteeringAngleRateAck
void setSteeringAngleRateAck(ObjectState &state, const double val, const bool reset_covariance=true)
Set the ackermann steering angle rate for a given object state.
Definition state_setters.h:435
perception_msgs::object_access::setWidth
void setWidth(ObjectState &state, const double val, const bool reset_covariance=true)
Set the width for a given object state.
Definition state_setters.h:516
perception_msgs::object_access::setTrafficLightType
void setTrafficLightType(ObjectState &state, const uint8_t val)
Set the traffic light type for a given object state.
Definition state_setters.h:645
perception_msgs::object_access::setAccLon
void setAccLon(ObjectState &state, const double val, const bool reset_covariance=true)
Set the longitudinal acceleration for a given object state.
Definition state_setters.h:183
perception_msgs::object_access::setVelLon
void setVelLon(ObjectState &state, const double val, const bool reset_covariance=true)
Set the longitdunial velocity for a given object state.
Definition state_setters.h:129
perception_msgs::object_access::setSteeringAngleAck
void setSteeringAngleAck(ObjectState &state, const double val, const bool reset_covariance=true)
Set the ackermann steering angle for a given object state.
Definition state_setters.h:408
perception_msgs::object_access::setX
void setX(ObjectState &state, const double val, const bool reset_covariance=true)
Set the x-position for a given object state.
Definition state_setters.h:48
perception_msgs::object_access::setY
void setY(ObjectState &state, const double val, const bool reset_covariance=true)
Set the y-position for a given object state.
Definition state_setters.h:75
perception_msgs::object_access::setVelLat
void setVelLat(ObjectState &state, const double val, const bool reset_covariance=true)
Set the lateral velocity for a given object state.
Definition state_setters.h:156
perception_msgs::object_access::setYaw
void setYaw(ObjectState &state, const double val, const bool reset_covariance=true)
Set the yaw for a given object state.
Definition state_setters.h:351
perception_msgs::object_access::setTrafficLightState
void setTrafficLightState(ObjectState &state, const uint8_t val)
Set the traffic light state for a given object state.
Definition state_setters.h:620
perception_msgs::object_access::setRoll
void setRoll(ObjectState &state, const double val, const bool reset_covariance=true)
Set the roll for a given object state.
Definition state_setters.h:237
perception_msgs::object_access::setLength
void setLength(ObjectState &state, const double val, const bool reset_covariance=true)
Set the length for a given object state.
Definition state_setters.h:543
perception_msgs::object_access::setPitch
void setPitch(ObjectState &state, const double val, const bool reset_covariance=true)
Set the pitch for a given object state.
Definition state_setters.h:294
perception_msgs::object_access::setPitchRate
void setPitchRate(ObjectState &state, const double val, const bool reset_covariance=true)
Set the pitch-rate for a given object state.
Definition state_setters.h:324
perception_msgs::object_access::setStandstill
void setStandstill(ObjectState &state, const bool val)
Set the standstill indication for a given object state.
Definition state_setters.h:596
perception_msgs::object_access::setZ
void setZ(ObjectState &state, const double val, const bool reset_covariance=true)
Set the z-position for a given object state.
Definition state_setters.h:102
perception_msgs::object_access::setSteeringAngleFront
void setSteeringAngleFront(ObjectState &state, const double val, const bool reset_covariance=true)
Set the front wheel angle for a given object state.
Definition state_setters.h:462
utils.h
Object state utility functions.
perception_msgs::object_access::setContinuousStateCovarianceToUnknownAt
void setContinuousStateCovarianceToUnknownAt(ObjectState &state, const unsigned int i, const unsigned int j)
Set the continuous state covariance to unknown at (i,j) for a given object state.
Definition utils.h:174
Generated by doxygen 1.9.8