perception_interfaces 1.1.2
Loading...
Searching...
No Matches
Functions
convenience_state_setters.h File Reference

Convenience setter functions for objects state members. More...

#include <perception_msgs_utils/impl/checks.h>
#include <perception_msgs_utils/impl/state_index.h>
#include <perception_msgs_utils/impl/state_setters.h>
#include <array>

Go to the source code of this file.

Functions

void perception_msgs::object_access::setContinuousState (ObjectState &state, const std::vector< double > &val)
 Set the continuous state of a given object state.
 
template<typename T >
void perception_msgs::object_access::setContinuousState (T &obj, const std::vector< double > &val)
 Set the continuous state of a given template object that contains an object state.
 
void perception_msgs::object_access::setDiscreteState (ObjectState &state, const std::vector< long int > &val)
 Set the discrete state of a given object state.
 
template<typename T >
void perception_msgs::object_access::setDiscreteState (T &obj, const std::vector< long int > &val)
 Set the discrete state of a given template object that contains an object state.
 
void perception_msgs::object_access::setContinuousStateCovariance (ObjectState &state, const std::vector< double > &val)
 Set the continuous state covariance of a given object state.
 
template<typename T >
void perception_msgs::object_access::setContinuousStateCovariance (T &obj, const std::vector< double > &val)
 Set the continuous state covariance of a given template object that contains an object state.
 
void perception_msgs::object_access::setContinuousStateCovarianceAt (ObjectState &state, const unsigned int i, const unsigned int j, const double val)
 Set the continuous state covariance entry at (i,j) of a given object state.
 
template<typename T >
void perception_msgs::object_access::setContinuousStateCovarianceAt (T &obj, const unsigned int i, const unsigned int j, const double val)
 Set the continuous state covariance at (i,j) of a given template object that contains an object state.
 
void perception_msgs::object_access::setContinuousStateCovarianceDiagonal (ObjectState &state, const std::vector< double > val)
 Set the continuous state covariance diagonal of a given object state.
 
template<typename T >
void perception_msgs::object_access::setContinuousStateCovarianceDiagonal (T &obj, const std::vector< double > val)
 Set the continuous state covariance diagonal of a given template object that contains an object state.
 
void perception_msgs::object_access::setPosition (ObjectState &state, const gm::Point val, const bool reset_covariance=true)
 Set the position of a given object state.
 
template<typename T >
void perception_msgs::object_access::setPosition (T &obj, const gm::Point val, const bool reset_covariance=true)
 Set the position of a given template object that contains an object state.
 
void perception_msgs::object_access::setPosition (ObjectState &state, const std::array< double, 3 > &val, const bool reset_covariance=true)
 Set the position of a given object state.
 
template<typename T >
void perception_msgs::object_access::setPosition (T &obj, const std::array< double, 3 > &val, const bool reset_covariance=true)
 Set the position of a given template object that contains an object state.
 
void perception_msgs::object_access::setOrientation (ObjectState &state, const gm::Quaternion &val, const bool reset_covariance=true)
 Set the orientation of a given object state.
 
template<typename T >
void perception_msgs::object_access::setOrientation (T &obj, const gm::Quaternion &val, const bool reset_covariance=true)
 Set the orientation of a given template object that contains an object state.
 
void perception_msgs::object_access::setOrientation (ObjectState &state, const std::array< double, 3 > &val, const bool reset_covariance=true)
 Set the orientation of a given object state.
 
template<typename T >
void perception_msgs::object_access::setOrientation (T &obj, const std::array< double, 3 > &val, const bool reset_covariance=true)
 Set the orientation of a given template object that contains an object state.
 
void perception_msgs::object_access::setPose (ObjectState &state, const gm::Pose &val, const bool reset_covariance=true)
 Set the pose of a given object state.
 
template<typename T >
void perception_msgs::object_access::setPose (T &obj, const gm::Pose &val, const bool reset_covariance=true)
 Set the pose of a given template object that contains an object state.
 
void perception_msgs::object_access::setPose (ObjectState &state, const std::array< double, 3 > &xyz, const std::array< double, 3 > &rpy, const bool reset_covariance=true)
 Set the pose of a given object state.
 
template<typename T >
void perception_msgs::object_access::setPose (T &obj, const std::array< double, 3 > &xyz, const std::array< double, 3 > &rpy, const bool reset_covariance=true)
 Set the pose of a given template object that contains an object state.
 
void perception_msgs::object_access::setPoseCovariance (ObjectState &state, const gm::PoseWithCovariance::_covariance_type &val)
 Set the pose covariance of a given object state.
 
template<typename T >
void perception_msgs::object_access::setPoseCovariance (T &obj, const gm::PoseWithCovariance::_covariance_type &val)
 Set the pose covariance of a given template object that contains an object state.
 
void perception_msgs::object_access::setPoseWithCovariance (ObjectState &state, const gm::PoseWithCovariance &val)
 Set the pose with covariance of a given object state.
 
template<typename T >
void perception_msgs::object_access::setPoseWithCovariance (T &obj, const gm::PoseWithCovariance &val)
 Set the pose with covariance of a given template object that contains an object state.
 
void perception_msgs::object_access::setVelocity (ObjectState &state, const gm::Vector3 &val, const bool reset_covariance=true)
 Set the velocity of a given object state.
 
template<typename T >
void perception_msgs::object_access::setVelocity (T &obj, const gm::Vector3 &val, const bool reset_covariance=true)
 Set the velocity of a given template object that contains an object state.
 
void perception_msgs::object_access::setVelocity (ObjectState &state, const std::array< double, 2 > &val, const bool reset_covariance=true)
 Set the velocity of a given object state.
 
template<typename T >
void perception_msgs::object_access::setVelocity (T &obj, const std::array< double, 2 > &val, const bool reset_covariance=true)
 Set the velocity of a given template object that contains an object state.
 
void perception_msgs::object_access::setAcceleration (ObjectState &state, const gm::Vector3 &val, const bool reset_covariance=true)
 Set the acceleration of a given object state.
 
template<typename T >
void perception_msgs::object_access::setAcceleration (T &obj, const gm::Vector3 &val, const bool reset_covariance=true)
 Set the acceleration of a given template object that contains an object state.
 
void perception_msgs::object_access::setAcceleration (ObjectState &state, const std::array< double, 2 > &val, const bool reset_covariance=true)
 Set the acceleration of a given object state.
 
template<typename T >
void perception_msgs::object_access::setAcceleration (T &obj, const std::array< double, 2 > &val, const bool reset_covariance=true)
 Set the acceleration of a given template object that contains an object state.
 
void perception_msgs::object_access::setRollInDeg (ObjectState &state, const double val, const bool reset_covariance=true)
 Set the roll in degree of a given object state.
 
template<typename T >
void perception_msgs::object_access::setRollInDeg (T &obj, const double val, const bool reset_covariance=true)
 Set the roll in deg of a given template object that contains an object state.
 
void perception_msgs::object_access::setPitchInDeg (ObjectState &state, const double val, const bool reset_covariance=true)
 Set the pitch in degree of a given object state.
 
template<typename T >
void perception_msgs::object_access::setPitchInDeg (T &obj, const double val, const bool reset_covariance=true)
 Set the pitch in degree of a given template object that contains an object state.
 
void perception_msgs::object_access::setYawInDeg (ObjectState &state, const double val, const bool reset_covariance=true)
 Set the yaw in degree of a given object state.
 
template<typename T >
void perception_msgs::object_access::setYawInDeg (T &obj, const double val, const bool reset_covariance=true)
 Set the yaw in degree of a given template object that contains an object state.
 
void perception_msgs::object_access::setVelocityXYZYawWithCovariance (ObjectState &state, const gm::Vector3 &vel_xyz_in, const double yaw, const double var_vel_x, const double var_vel_y, const double cov_vel_xy)
 Set the velocity XYZ and yaw with covariance of a given object state.
 
template<typename T >
void perception_msgs::object_access::setVelocityXYZYawWithCovariance (T &obj, const gm::Vector3 &vel_xyz_in, const double yaw, const double var_vel_x, const double var_vel_y, const double cov_vel_xy)
 Set the velocity XYZ and yaw with covariance of a given template object that contains an object state.
 
void perception_msgs::object_access::setVelocityXYZYaw (ObjectState &state, const gm::Vector3 &vel_xyz, const double yaw, const bool reset_covariance=true)
 Set the velocity XYZ and yaw of a given object state.
 
template<typename T >
void perception_msgs::object_access::setVelocityXYZYaw (T &obj, const gm::Vector3 &vel_xyz, const double yaw, const bool reset_covariance=true)
 Set the velocity XYZ and yaw of a given template object that contains an object state.
 
void perception_msgs::object_access::setAccelerationXYZYawWithCovariance (ObjectState &state, const gm::Vector3 &acc_xyz_in, const double yaw, const double var_acc_x, const double var_acc_y, const double cov_acc_xy)
 Set the acceleration XYZ and yaw with covariance of a given object state.
 
template<typename T >
void perception_msgs::object_access::setAccelerationXYZYawWithCovariance (T &obj, const gm::Vector3 &acc_xyz_in, const double yaw, const double var_acc_x, const double var_acc_y, const double cov_acc_xy)
 Set the acceleration XYZ and yaw with covariance of a given template object that contains an object state.
 
void perception_msgs::object_access::setAccelerationXYZYaw (ObjectState &state, const gm::Vector3 &acc_xyz, const double yaw, const bool reset_covariance=true)
 Set the acceleration XYZ and yaw of a given template object that contains an object state.
 
template<typename T >
void perception_msgs::object_access::setAccelerationXYZYaw (T &obj, const gm::Vector3 &acc_xyz, const double yaw, const bool reset_covariance=true)
 Set the acceleration XYZ and yaw of a given template object that contains an object state.
 

Detailed Description

Convenience setter functions for objects state members.

Definition in file convenience_state_setters.h.

Function Documentation

◆ setAcceleration() [1/4]

void perception_msgs::object_access::setAcceleration ( ObjectState &  state,
const gm::Vector3 &  val,
const bool  reset_covariance = true 
)
inline

Set the acceleration of a given object state.

Parameters
state
val
reset_covariance

Definition at line 471 of file convenience_state_setters.h.

472{
473 setAccLon(state, val.x, reset_covariance);
474 setAccLat(state, val.y, reset_covariance);
475}
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:189
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:162

◆ setAcceleration() [2/4]

void perception_msgs::object_access::setAcceleration ( ObjectState &  state,
const std::array< double, 2 > &  val,
const bool  reset_covariance = true 
)
inline

Set the acceleration of a given object state.

Parameters
state
val
reset_covariance

Definition at line 498 of file convenience_state_setters.h.

499{
500 setAccLon(state, val[0], reset_covariance);
501 setAccLat(state, val[1], reset_covariance);
502}

◆ setAcceleration() [3/4]

template<typename T >
void perception_msgs::object_access::setAcceleration ( T &  obj,
const gm::Vector3 &  val,
const bool  reset_covariance = true 
)
inline

Set the acceleration of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 486 of file convenience_state_setters.h.

487{
488 setAcceleration(obj.state, val, reset_covariance);
489}
perception_msgs::object_access::setAcceleration
void setAcceleration(ObjectState &state, const gm::Vector3 &val, const bool reset_covariance=true)
Set the acceleration of a given object state.
Definition convenience_state_setters.h:471

◆ setAcceleration() [4/4]

template<typename T >
void perception_msgs::object_access::setAcceleration ( T &  obj,
const std::array< double, 2 > &  val,
const bool  reset_covariance = true 
)
inline

Set the acceleration of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 513 of file convenience_state_setters.h.

514{
515 setAcceleration(obj.state, val, reset_covariance);
516}

◆ setAccelerationXYZYaw() [1/2]

void perception_msgs::object_access::setAccelerationXYZYaw ( ObjectState &  state,
const gm::Vector3 &  acc_xyz,
const double  yaw,
const bool  reset_covariance = true 
)
inline

Set the acceleration XYZ and yaw of a given template object that contains an object state.

Parameters
state
acc_xyz
yaw
reset_covariance

Definition at line 808 of file convenience_state_setters.h.

810{
811 gm::Vector3 acc_lon_lat, acc_xyz_in;
812 acc_xyz_in = acc_xyz;
813 tf2::Quaternion q;
814 q.setRPY(0.0, 0.0, -yaw);
815 gm::TransformStamped tf;
816 tf.transform.rotation = tf2::toMsg(q);
817 tf2::doTransform(acc_xyz_in, acc_lon_lat, tf);
818 setAcceleration(state, acc_lon_lat, reset_covariance);
819 setYaw(state, yaw, reset_covariance);
820}
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:330

◆ setAccelerationXYZYaw() [2/2]

template<typename T >
void perception_msgs::object_access::setAccelerationXYZYaw ( T &  obj,
const gm::Vector3 &  acc_xyz,
const double  yaw,
const bool  reset_covariance = true 
)
inline

Set the acceleration XYZ and yaw of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
acc_xyz
yaw
reset_covariance

Definition at line 832 of file convenience_state_setters.h.

833{
834 setAccelerationXYZYaw(obj.state, acc_xyz, yaw, reset_covariance);
835}
perception_msgs::object_access::setAccelerationXYZYaw
void setAccelerationXYZYaw(ObjectState &state, const gm::Vector3 &acc_xyz, const double yaw, const bool reset_covariance=true)
Set the acceleration XYZ and yaw of a given template object that contains an object state.
Definition convenience_state_setters.h:808

◆ setAccelerationXYZYawWithCovariance() [1/2]

void perception_msgs::object_access::setAccelerationXYZYawWithCovariance ( ObjectState &  state,
const gm::Vector3 &  acc_xyz_in,
const double  yaw,
const double  var_acc_x,
const double  var_acc_y,
const double  cov_acc_xy 
)
inline

Set the acceleration XYZ and yaw with covariance of a given object state.

Parameters
state
acc_xyz_in
yaw
var_acc_x
var_acc_y
cov_acc_xy

Definition at line 727 of file convenience_state_setters.h.

730{
731 gm::PoseWithCovariance acc_lon_lat, acc_xyz;
732 acc_xyz.pose.position.x = acc_xyz_in.x;
733 acc_xyz.pose.position.y = acc_xyz_in.y;
734 acc_xyz.pose.position.z = 0.0;
735
736 acc_xyz.covariance.at(0) = var_acc_x;
737 acc_xyz.covariance.at(1) = cov_acc_xy;
738 acc_xyz.covariance.at(6) = cov_acc_xy;
739 acc_xyz.covariance.at(7) = var_acc_y;
740
741 tf2::Quaternion q;
742 q.setRPY(0.0, 0.0, -yaw);
743 gm::TransformStamped tf;
744 tf.transform.rotation = tf2::toMsg(q);
745
746#ifdef ROS1
747 gm::PoseWithCovarianceStamped acc_lon_lat_stamped, acc_xyz_stamped;
748 acc_xyz_stamped.pose = acc_xyz;
749 tf2::doTransform(acc_xyz_stamped, acc_lon_lat_stamped, tf);
750 acc_lon_lat = acc_lon_lat_stamped.pose;
751#else
752 tf2::doTransform(acc_xyz, acc_lon_lat, tf);
753#endif
754
755 setAcceleration(state, {acc_lon_lat.pose.position.x, acc_lon_lat.pose.position.y}, false);
756 setYaw(state, yaw, false);
757
758 int ix, jx, n = 2;
759 auto model_id = state.model_id;
760 for (int i = 0; i < n; i++) {
761 for (int j = 0; j < n; j++) {
762 if (i == 0 && hasAccLon(model_id))
763 ix = indexAccLon(model_id);
764 else if (i == 1 && hasAccLat(model_id))
765 ix = indexAccLat(model_id);
766 else
767 continue;
768
769 if (j == 0 && hasAccLon(model_id))
770 jx = indexAccLon(model_id);
771 else if (j == 1 && hasAccLat(model_id))
772 jx = indexAccLat(model_id);
773 else
774 continue;
775
776 setContinuousStateCovarianceAt(state, ix, jx, acc_lon_lat.covariance.at(6 * i + j));
777 }
778 }
779}
perception_msgs::object_access::setContinuousStateCovarianceAt
void setContinuousStateCovarianceAt(ObjectState &state, const unsigned int i, const unsigned int j, const double val)
Set the continuous state covariance entry at (i,j) of a given object state.
Definition convenience_state_setters.h:108
perception_msgs::object_access::hasAccLat
bool hasAccLat(const unsigned char &model_id)
Indicates if given model contains a lateral acceleration.
Definition state_index.h:649
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:135
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:156
perception_msgs::object_access::hasAccLon
bool hasAccLon(const unsigned char &model_id)
Indicates if given model contains a longitudinal acceleration.
Definition state_index.h:627

◆ setAccelerationXYZYawWithCovariance() [2/2]

template<typename T >
void perception_msgs::object_access::setAccelerationXYZYawWithCovariance ( T &  obj,
const gm::Vector3 &  acc_xyz_in,
const double  yaw,
const double  var_acc_x,
const double  var_acc_y,
const double  cov_acc_xy 
)
inline

Set the acceleration XYZ and yaw with covariance of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
acc_xyz_in
yaw
var_acc_x
var_acc_y
cov_acc_xy

Definition at line 793 of file convenience_state_setters.h.

796{
797 return setAccelerationXYZYawWithCovariance(obj.state, acc_xyz_in, yaw, var_acc_x, var_acc_y, cov_acc_xy);
798}
perception_msgs::object_access::setAccelerationXYZYawWithCovariance
void setAccelerationXYZYawWithCovariance(ObjectState &state, const gm::Vector3 &acc_xyz_in, const double yaw, const double var_acc_x, const double var_acc_y, const double cov_acc_xy)
Set the acceleration XYZ and yaw with covariance of a given object state.
Definition convenience_state_setters.h:727

◆ setContinuousState() [1/2]

void perception_msgs::object_access::setContinuousState ( ObjectState &  state,
const std::vector< double > &  val 
)
inline

Set the continuous state of a given object state.

Parameters
state
val

Definition at line 31 of file convenience_state_setters.h.

32{
33 state.continuous_state = val;
34 sanityCheckContinuousState(state);
35}
perception_msgs::object_access::sanityCheckContinuousState
void sanityCheckContinuousState(const ObjectState &state)
Perform sanity check on continuous state of given object state.
Definition checks.h:63

◆ setContinuousState() [2/2]

template<typename T >
void perception_msgs::object_access::setContinuousState ( T &  obj,
const std::vector< double > &  val 
)
inline

Set the continuous state of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val

Definition at line 45 of file convenience_state_setters.h.

46{
47 setContinuousState(obj.state, val);
48}
perception_msgs::object_access::setContinuousState
void setContinuousState(ObjectState &state, const std::vector< double > &val)
Set the continuous state of a given object state.
Definition convenience_state_setters.h:31

◆ setContinuousStateCovariance() [1/2]

void perception_msgs::object_access::setContinuousStateCovariance ( ObjectState &  state,
const std::vector< double > &  val 
)
inline

Set the continuous state covariance of a given object state.

Parameters
state
val

Definition at line 81 of file convenience_state_setters.h.

82{
83 state.continuous_state_covariance = val;
84 sanityCheckContinuousStateCovariance(state);
85}
perception_msgs::object_access::sanityCheckContinuousStateCovariance
void sanityCheckContinuousStateCovariance(const ObjectState &state)
Perform sanity check on continuous state covariance of given object state.
Definition checks.h:103

◆ setContinuousStateCovariance() [2/2]

template<typename T >
void perception_msgs::object_access::setContinuousStateCovariance ( T &  obj,
const std::vector< double > &  val 
)
inline

Set the continuous state covariance of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val

Definition at line 95 of file convenience_state_setters.h.

96{
97 setContinuousStateCovariance(obj.state, val);
98}
perception_msgs::object_access::setContinuousStateCovariance
void setContinuousStateCovariance(ObjectState &state, const std::vector< double > &val)
Set the continuous state covariance of a given object state.
Definition convenience_state_setters.h:81

◆ setContinuousStateCovarianceAt() [1/2]

void perception_msgs::object_access::setContinuousStateCovarianceAt ( ObjectState &  state,
const unsigned int  i,
const unsigned int  j,
const double  val 
)
inline

Set the continuous state covariance entry at (i,j) of a given object state.

Parameters
state
i
j
val

Definition at line 108 of file convenience_state_setters.h.

110{
111 sanityCheckContinuousStateCovariance(state);
112 const int n = getContinuousStateSize(state);
113 state.continuous_state_covariance[n * i + j] = val;
114}
perception_msgs::object_access::getContinuousStateSize
int getContinuousStateSize(const ObjectState &state)
Get the continuous state size for a given object state.
Definition utils.h:30

◆ setContinuousStateCovarianceAt() [2/2]

template<typename T >
void perception_msgs::object_access::setContinuousStateCovarianceAt ( T &  obj,
const unsigned int  i,
const unsigned int  j,
const double  val 
)
inline

Set the continuous state covariance at (i,j) of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
i
j
val

Definition at line 126 of file convenience_state_setters.h.

128{
129 setContinuousStateCovarianceAt(obj.state, i, j, val);
130}

◆ setContinuousStateCovarianceDiagonal() [1/2]

void perception_msgs::object_access::setContinuousStateCovarianceDiagonal ( ObjectState &  state,
const std::vector< double >  val 
)
inline

Set the continuous state covariance diagonal of a given object state.

Parameters
state
val

Definition at line 138 of file convenience_state_setters.h.

139{
140 const int n = getContinuousStateSize(state);
141 for (int i = 0; i < n; i++) setContinuousStateCovarianceAt(state, i, i, val[i]);
142}

◆ setContinuousStateCovarianceDiagonal() [2/2]

template<typename T >
void perception_msgs::object_access::setContinuousStateCovarianceDiagonal ( T &  obj,
const std::vector< double >  val 
)
inline

Set the continuous state covariance diagonal of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val

Definition at line 152 of file convenience_state_setters.h.

153{
154 setContinuousStateCovarianceDiagonal(obj.state, val);
155}
perception_msgs::object_access::setContinuousStateCovarianceDiagonal
void setContinuousStateCovarianceDiagonal(ObjectState &state, const std::vector< double > val)
Set the continuous state covariance diagonal of a given object state.
Definition convenience_state_setters.h:138

◆ setDiscreteState() [1/2]

void perception_msgs::object_access::setDiscreteState ( ObjectState &  state,
const std::vector< long int > &  val 
)
inline

Set the discrete state of a given object state.

Parameters
state
val

Definition at line 56 of file convenience_state_setters.h.

57{
58 state.discrete_state = val;
59 sanityCheckDiscreteState(state);
60}
perception_msgs::object_access::sanityCheckDiscreteState
void sanityCheckDiscreteState(const ObjectState &state)
Perform sanity check on discrete state of given object state.
Definition checks.h:83

◆ setDiscreteState() [2/2]

template<typename T >
void perception_msgs::object_access::setDiscreteState ( T &  obj,
const std::vector< long int > &  val 
)
inline

Set the discrete state of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val

Definition at line 70 of file convenience_state_setters.h.

71{
72 setDiscreteState(obj.state, val);
73}
perception_msgs::object_access::setDiscreteState
void setDiscreteState(ObjectState &state, const std::vector< long int > &val)
Set the discrete state of a given object state.
Definition convenience_state_setters.h:56

◆ setOrientation() [1/4]

void perception_msgs::object_access::setOrientation ( ObjectState &  state,
const gm::Quaternion &  val,
const bool  reset_covariance = true 
)
inline

Set the orientation of a given object state.

Parameters
state
val
reset_covariance

Definition at line 222 of file convenience_state_setters.h.

223{
224 tf2::Quaternion q;
225 tf2::fromMsg(val, q);
226 double roll, pitch, yaw;
227 tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
228 if (hasRoll(state.model_id)) setRoll(state, roll, reset_covariance);
229 if (hasPitch(state.model_id)) setPitch(state, pitch, reset_covariance);
230 if (hasYaw(state.model_id)) setYaw(state, yaw, reset_covariance);
231}
perception_msgs::object_access::hasYaw
bool hasYaw(const unsigned char &model_id)
Indicates if given model contains a yaw.
Definition state_index.h:759
perception_msgs::object_access::hasRoll
bool hasRoll(const unsigned char &model_id)
Indicates if given model contains a roll.
Definition state_index.h:671
perception_msgs::object_access::hasPitch
bool hasPitch(const unsigned char &model_id)
Indicates if given model contains a pitch.
Definition state_index.h:715
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:216
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:273

◆ setOrientation() [2/4]

void perception_msgs::object_access::setOrientation ( ObjectState &  state,
const std::array< double, 3 > &  val,
const bool  reset_covariance = true 
)
inline

Set the orientation of a given object state.

Parameters
state
val
reset_covariance

Definition at line 254 of file convenience_state_setters.h.

255{
256 if (hasRoll(state.model_id)) setRoll(state, val[0], reset_covariance);
257 if (hasPitch(state.model_id)) setPitch(state, val[1], reset_covariance);
258 if (hasYaw(state.model_id)) setYaw(state, val[2], reset_covariance);
259}

◆ setOrientation() [3/4]

template<typename T >
void perception_msgs::object_access::setOrientation ( T &  obj,
const gm::Quaternion &  val,
const bool  reset_covariance = true 
)
inline

Set the orientation of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 242 of file convenience_state_setters.h.

243{
244 setOrientation(obj.state, val, reset_covariance);
245}
perception_msgs::object_access::setOrientation
void setOrientation(ObjectState &state, const gm::Quaternion &val, const bool reset_covariance=true)
Set the orientation of a given object state.
Definition convenience_state_setters.h:222

◆ setOrientation() [4/4]

template<typename T >
void perception_msgs::object_access::setOrientation ( T &  obj,
const std::array< double, 3 > &  val,
const bool  reset_covariance = true 
)
inline

Set the orientation of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 270 of file convenience_state_setters.h.

271{
272 setOrientation(obj.state, val, reset_covariance);
273}

◆ setPitchInDeg() [1/2]

void perception_msgs::object_access::setPitchInDeg ( ObjectState &  state,
const double  val,
const bool  reset_covariance = true 
)
inline

Set the pitch in degree of a given object state.

Parameters
state
val
reset_covariance

Definition at line 553 of file convenience_state_setters.h.

554{
555 setPitch(state, val * M_PI / 180.0, reset_covariance);
556}

◆ setPitchInDeg() [2/2]

template<typename T >
void perception_msgs::object_access::setPitchInDeg ( T &  obj,
const double  val,
const bool  reset_covariance = true 
)
inline

Set the pitch in degree of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 567 of file convenience_state_setters.h.

568{
569 setPitchInDeg(obj.state, val, reset_covariance);
570}
perception_msgs::object_access::setPitchInDeg
void setPitchInDeg(ObjectState &state, const double val, const bool reset_covariance=true)
Set the pitch in degree of a given object state.
Definition convenience_state_setters.h:553

◆ setPose() [1/4]

void perception_msgs::object_access::setPose ( ObjectState &  state,
const gm::Pose &  val,
const bool  reset_covariance = true 
)
inline

Set the pose of a given object state.

Parameters
state
val
reset_covariance

Definition at line 282 of file convenience_state_setters.h.

283{
284 setPosition(state, val.position, reset_covariance);
285 setOrientation(state, val.orientation, reset_covariance);
286}
perception_msgs::object_access::setPosition
void setPosition(ObjectState &state, const gm::Point val, const bool reset_covariance=true)
Set the position of a given object state.
Definition convenience_state_setters.h:166

◆ setPose() [2/4]

void perception_msgs::object_access::setPose ( ObjectState &  state,
const std::array< double, 3 > &  xyz,
const std::array< double, 3 > &  rpy,
const bool  reset_covariance = true 
)
inline

Set the pose of a given object state.

Parameters
state
xyz
rpy
reset_covariance

Definition at line 310 of file convenience_state_setters.h.

312{
313 setPosition(state, xyz, reset_covariance);
314 setOrientation(state, rpy, reset_covariance);
315}

◆ setPose() [3/4]

template<typename T >
void perception_msgs::object_access::setPose ( T &  obj,
const gm::Pose &  val,
const bool  reset_covariance = true 
)
inline

Set the pose of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 297 of file convenience_state_setters.h.

298{
299 setPose(obj.state, val, reset_covariance);
300}
perception_msgs::object_access::setPose
void setPose(ObjectState &state, const gm::Pose &val, const bool reset_covariance=true)
Set the pose of a given object state.
Definition convenience_state_setters.h:282

◆ setPose() [4/4]

template<typename T >
void perception_msgs::object_access::setPose ( T &  obj,
const std::array< double, 3 > &  xyz,
const std::array< double, 3 > &  rpy,
const bool  reset_covariance = true 
)
inline

Set the pose of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
xyz
rpy
reset_covariance

Definition at line 327 of file convenience_state_setters.h.

328{
329 setPose(obj.state, xyz, rpy, reset_covariance);
330}

◆ setPoseCovariance() [1/2]

void perception_msgs::object_access::setPoseCovariance ( ObjectState &  state,
const gm::PoseWithCovariance::_covariance_type &  val 
)
inline

Set the pose covariance of a given object state.

Parameters
state
val

Definition at line 338 of file convenience_state_setters.h.

340{
341 const int n = 6;
342 const int model_id = state.model_id;
343
344 int ix, jx;
345 for (int i = 0; i < n; i++) {
346 bool i_valid = true;
347 switch (i) {
348 case 0: i_valid = hasX(model_id); ix = i_valid ? indexX(model_id) : 0; break;
349 case 1: i_valid = hasY(model_id); ix = i_valid ? indexY(model_id) : 0; break;
350 case 2: i_valid = hasZ(model_id); ix = i_valid ? indexZ(model_id) : 0; break;
351 case 3: i_valid = hasRoll(model_id); ix = i_valid ? indexRoll(model_id) : 0; break;
352 case 4: i_valid = hasPitch(model_id); ix = i_valid ? indexPitch(model_id) : 0; break;
353 case 5: i_valid = hasYaw(model_id); ix = i_valid ? indexYaw(model_id) : 0; break;
354 }
355 if (!i_valid) continue;
356
357 for (int j = 0; j < n; j++) {
358 bool j_valid = true;
359 switch (j) {
360 case 0: j_valid = hasX(model_id); jx = j_valid ? indexX(model_id) : 0; break;
361 case 1: j_valid = hasY(model_id); jx = j_valid ? indexY(model_id) : 0; break;
362 case 2: j_valid = hasZ(model_id); jx = j_valid ? indexZ(model_id) : 0; break;
363 case 3: j_valid = hasRoll(model_id); jx = j_valid ? indexRoll(model_id) : 0; break;
364 case 4: j_valid = hasPitch(model_id); jx = j_valid ? indexPitch(model_id) : 0; break;
365 case 5: j_valid = hasYaw(model_id); jx = j_valid ? indexYaw(model_id) : 0; break;
366 }
367 if (!j_valid) continue;
368
369 setContinuousStateCovarianceAt(state, ix, jx, val[n * i + j]);
370 }
371 }
372}
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:177
perception_msgs::object_access::hasZ
bool hasZ(const unsigned char &model_id)
Indicates if given model contains a z-position.
Definition state_index.h:559
perception_msgs::object_access::hasY
bool hasY(const unsigned char &model_id)
Indicates if given model contains a y-position.
Definition state_index.h:535
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:47
perception_msgs::object_access::hasX
bool hasX(const unsigned char &model_id)
Indicates if given model contains an x-position.
Definition state_index.h:511
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:70
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:211
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:24
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:245

◆ setPoseCovariance() [2/2]

template<typename T >
void perception_msgs::object_access::setPoseCovariance ( T &  obj,
const gm::PoseWithCovariance::_covariance_type &  val 
)
inline

Set the pose covariance of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val

Definition at line 381 of file convenience_state_setters.h.

382{
383 setPoseCovariance(obj.state, val);
384}
perception_msgs::object_access::setPoseCovariance
void setPoseCovariance(ObjectState &state, const gm::PoseWithCovariance::_covariance_type &val)
Set the pose covariance of a given object state.
Definition convenience_state_setters.h:338

◆ setPoseWithCovariance() [1/2]

void perception_msgs::object_access::setPoseWithCovariance ( ObjectState &  state,
const gm::PoseWithCovariance &  val 
)
inline

Set the pose with covariance of a given object state.

Parameters
state
val

Definition at line 392 of file convenience_state_setters.h.

393{
394 setPose(state, val.pose, false);
395 setPoseCovariance(state, val.covariance);
396}

◆ setPoseWithCovariance() [2/2]

template<typename T >
void perception_msgs::object_access::setPoseWithCovariance ( T &  obj,
const gm::PoseWithCovariance &  val 
)
inline

Set the pose with covariance of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val

Definition at line 406 of file convenience_state_setters.h.

407{
408 setPoseWithCovariance(obj.state, val);
409}
perception_msgs::object_access::setPoseWithCovariance
void setPoseWithCovariance(ObjectState &state, const gm::PoseWithCovariance &val)
Set the pose with covariance of a given object state.
Definition convenience_state_setters.h:392

◆ setPosition() [1/4]

void perception_msgs::object_access::setPosition ( ObjectState &  state,
const gm::Point  val,
const bool  reset_covariance = true 
)
inline

Set the position of a given object state.

Parameters
state
val
reset_covariance

Definition at line 166 of file convenience_state_setters.h.

167{
168 setX(state, val.x, reset_covariance);
169 setY(state, val.y, reset_covariance);
170 setZ(state, val.z, reset_covariance);
171}
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:27
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:54
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:81

◆ setPosition() [2/4]

void perception_msgs::object_access::setPosition ( ObjectState &  state,
const std::array< double, 3 > &  val,
const bool  reset_covariance = true 
)
inline

Set the position of a given object state.

Parameters
state
val
reset_covariance

Definition at line 194 of file convenience_state_setters.h.

195{
196 setX(state, val[0], reset_covariance);
197 setY(state, val[1], reset_covariance);
198 setZ(state, val[2], reset_covariance);
199}

◆ setPosition() [3/4]

template<typename T >
void perception_msgs::object_access::setPosition ( T &  obj,
const gm::Point  val,
const bool  reset_covariance = true 
)
inline

Set the position of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 182 of file convenience_state_setters.h.

183{
184 setPosition(obj.state, val, reset_covariance);
185}

◆ setPosition() [4/4]

template<typename T >
void perception_msgs::object_access::setPosition ( T &  obj,
const std::array< double, 3 > &  val,
const bool  reset_covariance = true 
)
inline

Set the position of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 210 of file convenience_state_setters.h.

211{
212 setPosition(obj.state, val, reset_covariance);
213}

◆ setRollInDeg() [1/2]

void perception_msgs::object_access::setRollInDeg ( ObjectState &  state,
const double  val,
const bool  reset_covariance = true 
)
inline

Set the roll in degree of a given object state.

Parameters
state
val
reset_covariance

Definition at line 527 of file convenience_state_setters.h.

528{
529 setRoll(state, val * M_PI / 180.0, reset_covariance);
530}

◆ setRollInDeg() [2/2]

template<typename T >
void perception_msgs::object_access::setRollInDeg ( T &  obj,
const double  val,
const bool  reset_covariance = true 
)
inline

Set the roll in deg of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 541 of file convenience_state_setters.h.

542{
543 setRollInDeg(obj.state, val, reset_covariance);
544}
perception_msgs::object_access::setRollInDeg
void setRollInDeg(ObjectState &state, const double val, const bool reset_covariance=true)
Set the roll in degree of a given object state.
Definition convenience_state_setters.h:527

◆ setVelocity() [1/4]

void perception_msgs::object_access::setVelocity ( ObjectState &  state,
const gm::Vector3 &  val,
const bool  reset_covariance = true 
)
inline

Set the velocity of a given object state.

Parameters
state
val
reset_covariance

Definition at line 418 of file convenience_state_setters.h.

419{
420 setVelLon(state, val.x, reset_covariance);
421 setVelLat(state, val.y, reset_covariance);
422}
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:108
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:135

◆ setVelocity() [2/4]

void perception_msgs::object_access::setVelocity ( ObjectState &  state,
const std::array< double, 2 > &  val,
const bool  reset_covariance = true 
)
inline

Set the velocity of a given object state.

Parameters
state
val
reset_covariance

Definition at line 444 of file convenience_state_setters.h.

445{
446 setVelLon(state, val[0], reset_covariance);
447 setVelLat(state, val[1], reset_covariance);
448}

◆ setVelocity() [3/4]

template<typename T >
void perception_msgs::object_access::setVelocity ( T &  obj,
const gm::Vector3 &  val,
const bool  reset_covariance = true 
)
inline

Set the velocity of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 432 of file convenience_state_setters.h.

433{
434 setVelocity(obj.state, val, reset_covariance);
435}
perception_msgs::object_access::setVelocity
void setVelocity(ObjectState &state, const gm::Vector3 &val, const bool reset_covariance=true)
Set the velocity of a given object state.
Definition convenience_state_setters.h:418

◆ setVelocity() [4/4]

template<typename T >
void perception_msgs::object_access::setVelocity ( T &  obj,
const std::array< double, 2 > &  val,
const bool  reset_covariance = true 
)
inline

Set the velocity of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 459 of file convenience_state_setters.h.

460{
461 setVelocity(obj.state, val, reset_covariance);
462}

◆ setVelocityXYZYaw() [1/2]

void perception_msgs::object_access::setVelocityXYZYaw ( ObjectState &  state,
const gm::Vector3 &  vel_xyz,
const double  yaw,
const bool  reset_covariance = true 
)
inline

Set the velocity XYZ and yaw of a given object state.

Parameters
state
vel_xyz
yaw
reset_covariance

Definition at line 689 of file convenience_state_setters.h.

690{
691 gm::Vector3 vel_lon_lat, vel_xyz_in;
692 vel_xyz_in = vel_xyz;
693 tf2::Quaternion q;
694 q.setRPY(0.0, 0.0, -yaw);
695 gm::TransformStamped tf;
696 tf.transform.rotation = tf2::toMsg(q);
697 tf2::doTransform(vel_xyz_in, vel_lon_lat, tf);
698 setVelocity(state, vel_lon_lat, reset_covariance);
699 setYaw(state, yaw, reset_covariance);
700}

◆ setVelocityXYZYaw() [2/2]

template<typename T >
void perception_msgs::object_access::setVelocityXYZYaw ( T &  obj,
const gm::Vector3 &  vel_xyz,
const double  yaw,
const bool  reset_covariance = true 
)
inline

Set the velocity XYZ and yaw of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
vel_xyz
yaw
reset_covariance

Definition at line 712 of file convenience_state_setters.h.

713{
714 setVelocityXYZYaw(obj.state, vel_xyz, yaw, reset_covariance);
715}
perception_msgs::object_access::setVelocityXYZYaw
void setVelocityXYZYaw(ObjectState &state, const gm::Vector3 &vel_xyz, const double yaw, const bool reset_covariance=true)
Set the velocity XYZ and yaw of a given object state.
Definition convenience_state_setters.h:689

◆ setVelocityXYZYawWithCovariance() [1/2]

void perception_msgs::object_access::setVelocityXYZYawWithCovariance ( ObjectState &  state,
const gm::Vector3 &  vel_xyz_in,
const double  yaw,
const double  var_vel_x,
const double  var_vel_y,
const double  cov_vel_xy 
)
inline

Set the velocity XYZ and yaw with covariance of a given object state.

Parameters
state
vel_xyz_in
yaw
var_vel_x
var_vel_y
cov_vel_xy

Definition at line 608 of file convenience_state_setters.h.

611{
612 gm::PoseWithCovariance vel_lon_lat, vel_xyz;
613 vel_xyz.pose.position.x = vel_xyz_in.x;
614 vel_xyz.pose.position.y = vel_xyz_in.y;
615 vel_xyz.pose.position.z = 0.0;
616
617 vel_xyz.covariance.at(0) = var_vel_x;
618 vel_xyz.covariance.at(1) = cov_vel_xy;
619 vel_xyz.covariance.at(6) = cov_vel_xy;
620 vel_xyz.covariance.at(7) = var_vel_y;
621
622 tf2::Quaternion q;
623 q.setRPY(0.0, 0.0, -yaw);
624 gm::TransformStamped tf;
625 tf.transform.rotation = tf2::toMsg(q);
626
627#ifdef ROS1
628 gm::PoseWithCovarianceStamped vel_lon_lat_stamped, vel_xyz_stamped;
629 vel_xyz_stamped.pose = vel_xyz;
630 tf2::doTransform(vel_xyz_stamped, vel_lon_lat_stamped, tf);
631 vel_lon_lat = vel_lon_lat_stamped.pose;
632#else
633 tf2::doTransform(vel_xyz, vel_lon_lat, tf);
634#endif
635
636 setVelocity(state, {vel_lon_lat.pose.position.x, vel_lon_lat.pose.position.y}, false);
637 setYaw(state, yaw, false);
638
639 int ix, jx, n = 2;
640 auto model_id = state.model_id;
641 for (int i = 0; i < n; i++) {
642 for (int j = 0; j < n; j++) {
643 if (i == 0 && hasVelLon(model_id))
644 ix = indexVelLon(model_id);
645 else if (i == 1 && hasVelLat(model_id))
646 ix = indexVelLat(model_id);
647 else
648 continue;
649
650 if (j == 0 && hasVelLon(model_id))
651 jx = indexVelLon(model_id);
652 else if (j == 1 && hasVelLat(model_id))
653 jx = indexVelLat(model_id);
654 else
655 continue;
656
657 setContinuousStateCovarianceAt(state, ix, jx, vel_lon_lat.covariance.at(6 * i + j));
658 }
659 }
660}
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:93
perception_msgs::object_access::hasVelLat
bool hasVelLat(const unsigned char &model_id)
Indicates if given model contains a lateral velocity.
Definition state_index.h:605
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:114
perception_msgs::object_access::hasVelLon
bool hasVelLon(const unsigned char &model_id)
Indicates if given model contains a longitudinal velocity.
Definition state_index.h:583

◆ setVelocityXYZYawWithCovariance() [2/2]

template<typename T >
void perception_msgs::object_access::setVelocityXYZYawWithCovariance ( T &  obj,
const gm::Vector3 &  vel_xyz_in,
const double  yaw,
const double  var_vel_x,
const double  var_vel_y,
const double  cov_vel_xy 
)
inline

Set the velocity XYZ and yaw with covariance of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
vel_xyz_in
yaw
var_vel_x
var_vel_y
cov_vel_xy

Definition at line 674 of file convenience_state_setters.h.

677{
678 return setVelocityXYZYawWithCovariance(obj.state, vel_xyz_in, yaw, var_vel_x, var_vel_y, cov_vel_xy);
679}
perception_msgs::object_access::setVelocityXYZYawWithCovariance
void setVelocityXYZYawWithCovariance(ObjectState &state, const gm::Vector3 &vel_xyz_in, const double yaw, const double var_vel_x, const double var_vel_y, const double cov_vel_xy)
Set the velocity XYZ and yaw with covariance of a given object state.
Definition convenience_state_setters.h:608

◆ setYawInDeg() [1/2]

void perception_msgs::object_access::setYawInDeg ( ObjectState &  state,
const double  val,
const bool  reset_covariance = true 
)
inline

Set the yaw in degree of a given object state.

Parameters
state
val
reset_covariance

Definition at line 579 of file convenience_state_setters.h.

580{
581 setYaw(state, val * M_PI / 180.0, reset_covariance);
582}

◆ setYawInDeg() [2/2]

template<typename T >
void perception_msgs::object_access::setYawInDeg ( T &  obj,
const double  val,
const bool  reset_covariance = true 
)
inline

Set the yaw in degree of a given template object that contains an object state.

Template Parameters
T
Parameters
obj
val
reset_covariance

Definition at line 593 of file convenience_state_setters.h.

594{
595 setYawInDeg(obj.state, val, reset_covariance);
596}
perception_msgs::object_access::setYawInDeg
void setYawInDeg(ObjectState &state, const double val, const bool reset_covariance=true)
Set the yaw in degree of a given object state.
Definition convenience_state_setters.h:579
Generated by doxygen 1.9.8