convenience_state_getters.h File Reference

Convenience getter functions for object state members. More...

#include <perception_msgs_utils/impl/checks.h>
#include <perception_msgs_utils/impl/state_getters.h>
#include <perception_msgs_utils/impl/state_index.h>
#include <algorithm>
#include <cmath>

Go to the source code of this file.

Functions
std::vector< double >	perception_msgs::object_access::getContinuousState (const ObjectState &state)
	Get the continuous state for a given object state.
template<typename T >
std::vector< double >	perception_msgs::object_access::getContinuousState (const T &obj)
	Get the continuous state for a given template object that contains an object state.
std::vector< long int >	perception_msgs::object_access::getDiscreteState (const ObjectState &state)
	Get the discrete state for a given object state.
template<typename T >
std::vector< long int >	perception_msgs::object_access::getDiscreteState (const T &obj)
	Get the discrete state for a given template object that contains an object state.
std::vector< double >	perception_msgs::object_access::getContinuousStateCovariance (const ObjectState &state)
	Get the continuous state covariance for a given object state.
template<typename T >
std::vector< double >	perception_msgs::object_access::getContinuousStateCovariance (const T &obj)
	Get the continuous state covariance for a given template object that contains an object state.
double	perception_msgs::object_access::getContinuousStateCovarianceAt (const ObjectState &state, const unsigned int i, const unsigned int j)
	Get the continuous state covariance entry (i,j) for a given object state.
template<typename T >
double	perception_msgs::object_access::getContinuousStateCovarianceAt (const T &obj, const unsigned int i, const unsigned int j)
	Get the continuous state covariance entry (i,j) for a given template object that contains an object state.
std::vector< double >	perception_msgs::object_access::getContinuousStateCovarianceDiagonal (const ObjectState &state)
	Get the continuous state covariance diagonal for a given object state.
template<typename T >
std::vector< double >	perception_msgs::object_access::getContinuousStateCovarianceDiagonal (const T &obj)
	Get the continuous state covariance diagonal for a given template object that contains an object state.
gm::Point	perception_msgs::object_access::getPosition (const ObjectState &state)
	Get the position of a given object state.
template<typename T >
gm::Point	perception_msgs::object_access::getPosition (const T &obj)
	Get the position of a given template object that contains an object state.
gm::Quaternion	perception_msgs::object_access::getOrientation (const ObjectState &state)
	Get the orientation of a given object state.
template<typename T >
gm::Quaternion	perception_msgs::object_access::getOrientation (const T &obj)
	Get the orientation of a given template object that contains an object state.
gm::Pose	perception_msgs::object_access::getPose (const ObjectState &state)
	Get the pose of a given object state.
template<typename T >
gm::Pose	perception_msgs::object_access::getPose (const T &obj)
	Get the pose of a given template object that contains an object state.
std::vector< double >	perception_msgs::object_access::getPoseCovariance (const ObjectState &state)
	Get the pose covariance of a given object state.
template<typename T >
std::vector< double >	perception_msgs::object_access::getPoseCovariance (const T &obj)
	Get the pose covariance of a given template object that contains an object state.
gm::PoseWithCovariance	perception_msgs::object_access::getPoseWithCovariance (const ObjectState &state)
	Get the pose with covariance of a given object state.
template<typename T >
gm::PoseWithCovariance	perception_msgs::object_access::getPoseWithCovariance (const T &obj)
	Get the pose with covariance of a given template object that contains an object state.
gm::Vector3	perception_msgs::object_access::getVelocity (const ObjectState &state)
	Get the velocity of a given object state.
template<typename T >
gm::Vector3	perception_msgs::object_access::getVelocity (const T &obj)
	Get the velocity of a given template object that contains an object state.
double	perception_msgs::object_access::getVelocityMagnitude (const ObjectState &state)
	Get the velocity magnitude of a given object state.
template<typename T >
double	perception_msgs::object_access::getVelocityMagnitude (const T &obj)
	Get the velocity magnitude of a given template object that contains an object state.
gm::Vector3	perception_msgs::object_access::getAcceleration (const ObjectState &state)
	Get the acceleration of a given object state.
template<typename T >
gm::Vector3	perception_msgs::object_access::getAcceleration (const T &obj)
	Get the acceleration of a given template object that contains an object state.
double	perception_msgs::object_access::getAccelerationMagnitude (const ObjectState &state)
	Get the acceleration magnitude of a given object state.
template<typename T >
double	perception_msgs::object_access::getAccelerationMagnitude (const T &obj)
	Get the acceleration magnitude of a given template object that contains an object state.
double	perception_msgs::object_access::getRollInDeg (const ObjectState &state)
	Get the roll in degree of a given object state.
template<typename T >
double	perception_msgs::object_access::getRollInDeg (const T &obj)
	Get the roll in degree of a given template object that contains an object state.
double	perception_msgs::object_access::getPitchInDeg (const ObjectState &state)
	Get the pitch in degree of a given object state.
template<typename T >
double	perception_msgs::object_access::getPitchInDeg (const T &obj)
	Get the pitch in degree of a given template object that contains an object state.
double	perception_msgs::object_access::getYawInDeg (const ObjectState &state)
	Get the yaw in degree of a given object state.
template<typename T >
double	perception_msgs::object_access::getYawInDeg (const T &obj)
	Get the yaw in degree of a given template object that contains an object state.
gm::PoseWithCovariance	perception_msgs::object_access::getVelocityXYZWithCovariance (const ObjectState &state)
	Get the velocity XYZ with covariance of a object state.
template<typename T >
gm::PoseWithCovariance	perception_msgs::object_access::getVelocityXYZWithCovariance (const T &obj)
	Get the velocity XYZ with covariance of a given template object that contains an object state.
gm::Vector3	perception_msgs::object_access::getVelocityXYZ (const ObjectState &state)
	Get the velocity XYZ of a given object state.
template<typename T >
gm::Vector3	perception_msgs::object_access::getVelocityXYZ (const T &obj)
	Get the velocity XYZ of a given template object that contains an object state.
double	perception_msgs::object_access::getVelX (const ObjectState &state)
	Get the x-velocity of a given object state.
template<typename T >
double	perception_msgs::object_access::getVelX (const T &obj)
	Get the x-velocity of a given template object that contains an object state.
double	perception_msgs::object_access::getVelY (const ObjectState &state)
	Get the y-velocity of a given object state.
template<typename T >
double	perception_msgs::object_access::getVelY (const T &obj)
	Get the y-velocity of a given template object that contains an object state.
gm::PoseWithCovariance	perception_msgs::object_access::getAccelerationXYZWithCovariance (const ObjectState &state)
	Get the acceleration XYZ with covariance of a given object state.
template<typename T >
gm::PoseWithCovariance	perception_msgs::object_access::getAccelerationXYZWithCovariance (const T &obj)
	Get the acceleration XYZ with covariance of a given template object that contains an object state.
gm::Vector3	perception_msgs::object_access::getAccelerationXYZ (const ObjectState &state)
	Get the acceleration XYZ of a given object state.
template<typename T >
gm::Vector3	perception_msgs::object_access::getAccelerationXYZ (const T &obj)
	Get the acceleration XYZ of a given template object that contains an object state.
double	perception_msgs::object_access::getAccX (const ObjectState &state)
	Get the x-acceleration of a given object state.
template<typename T >
double	perception_msgs::object_access::getAccX (const T &obj)
	Get the x-acceleration of a given template object that contains an object state.
double	perception_msgs::object_access::getAccY (const ObjectState &state)
	Get the y-acceleration of a given object state.
template<typename T >
double	perception_msgs::object_access::getAccY (const T &obj)
	Get the y-acceleration of a given template object that contains an object state.
ObjectClassification	perception_msgs::object_access::getClassWithHighestProbability (const ObjectState &state)
	Get the classification with highest probability of a given object state.
template<typename T >
ObjectClassification	perception_msgs::object_access::getClassWithHighestProbability (const T &obj)
	Get the classification with highest probability of a given template object that contains an object state.

Detailed Description

Convenience getter functions for object state members.

============================================================================ MIT License

Copyright (c) 2025 Institute for Automotive Engineering (ika), RWTH Aachen University

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE

SOFTWARE.

Definition in file convenience_state_getters.h.

Function Documentation

getAcceleration() [1/2]

gm::Vector3 perception_msgs::object_access::getAcceleration ( const ObjectState &  state )

inline

Get the acceleration of a given object state.

Parameters

state

Returns

gm::Vector3

Definition at line 422 of file convenience_state_getters.h.

{
  gm::Vector3 acceleration;
  acceleration.x = getAccLon(state);
  acceleration.y = getAccLat(state);
  acceleration.z = 0.0;
  return acceleration;
}

getAcceleration() [2/2]

template<typename T >

gm::Vector3 perception_msgs::object_access::getAcceleration ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::Vector3

Definition at line 439 of file convenience_state_getters.h.

{
  return getAcceleration(obj.state);
}

getAccelerationMagnitude() [1/2]

double perception_msgs::object_access::getAccelerationMagnitude ( const ObjectState &  state )

inline

Get the acceleration magnitude of a given object state.

Parameters

state

Returns

double

Definition at line 450 of file convenience_state_getters.h.

{
  gm::Vector3 acc = getAcceleration(state);
  using namespace std;
  return sqrt(pow(acc.x, 2) + pow(acc.y, 2) + pow(acc.z, 2));
}

getAccelerationMagnitude() [2/2]

template<typename T >

double perception_msgs::object_access::getAccelerationMagnitude ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 465 of file convenience_state_getters.h.

{
  return getAccelerationMagnitude(obj.state);
}

getAccelerationXYZ() [1/2]

gm::Vector3 perception_msgs::object_access::getAccelerationXYZ ( const ObjectState &  state )

inline

Get the acceleration XYZ of a given object state.

Parameters

state

Returns

gm::Vector3

Definition at line 743 of file convenience_state_getters.h.

{
  gm::Vector3 acc_lon_lat, acc_xyz;
  acc_lon_lat = getAcceleration(state);
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, getYaw(state));
  gm::TransformStamped tf;
  tf.transform.rotation = tf2::toMsg(q);
  tf2::doTransform(acc_lon_lat, acc_xyz, tf);
  return acc_xyz;
}

getAccelerationXYZ() [2/2]

template<typename T >

gm::Vector3 perception_msgs::object_access::getAccelerationXYZ ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::Vector3

Definition at line 763 of file convenience_state_getters.h.

{
  return getAccelerationXYZ(obj.state);
}

getAccelerationXYZWithCovariance() [1/2]

gm::PoseWithCovariance perception_msgs::object_access::getAccelerationXYZWithCovariance ( const ObjectState &  state )

inline

Get the acceleration XYZ with covariance of a given object state.

Parameters

state

Returns

gm::PoseWithCovariance

Definition at line 680 of file convenience_state_getters.h.

{
  gm::PoseWithCovariance acc_lon_lat, acc_xyz;
  acc_lon_lat.pose.position.x = getAccLon(state);
  acc_lon_lat.pose.position.y = getAccLat(state);
  acc_lon_lat.pose.position.z = 0.0;
 
  int ix, jx, n = 2;
  auto model_id = state.model_id;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (i == 0 && hasAccLon(model_id))
        ix = indexAccLon(model_id);
      else if (i == 1 && hasAccLat(model_id))
        ix = indexAccLat(model_id);
      else
        continue;
 
      if (j == 0 && hasAccLon(model_id))
        jx = indexAccLon(model_id);
      else if (j == 1 && hasAccLat(model_id))
        jx = indexAccLat(model_id);
      else
        continue;
 
      acc_lon_lat.covariance.at(6 * i + j) = getContinuousStateCovarianceAt(state, ix, jx);
    }
  }
 
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, getYaw(state));
  gm::TransformStamped tf;
  tf.transform.rotation = tf2::toMsg(q);
#ifdef ROS1
  gm::PoseWithCovarianceStamped acc_lon_lat_stamped, acc_xyz_stamped;
  acc_lon_lat_stamped.pose = acc_lon_lat;
  tf2::doTransform(acc_lon_lat_stamped, acc_xyz_stamped, tf);
  acc_xyz = acc_xyz_stamped.pose;
#else
  tf2::doTransform(acc_lon_lat, acc_xyz, tf);
#endif
  return acc_xyz;
}

getAccelerationXYZWithCovariance() [2/2]

template<typename T >

gm::PoseWithCovariance perception_msgs::object_access::getAccelerationXYZWithCovariance ( const T &  obj )

inline

Get the acceleration XYZ with covariance of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

gm::PoseWithCovariance

Definition at line 732 of file convenience_state_getters.h.

{
  return getAccelerationXYZWithCovariance(obj.state);
}

getAccX() [1/2]

double perception_msgs::object_access::getAccX ( const ObjectState &  state )

inline

Get the x-acceleration of a given object state.

Parameters

state

Returns

double

Definition at line 774 of file convenience_state_getters.h.

{ return getAccelerationXYZ(state).x; }

getAccX() [2/2]

template<typename T >

double perception_msgs::object_access::getAccX ( const T &  obj )

inline

Get the x-acceleration of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 784 of file convenience_state_getters.h.

{
  return getAccX(obj.state);
}

getAccY() [1/2]

double perception_msgs::object_access::getAccY ( const ObjectState &  state )

inline

Get the y-acceleration of a given object state.

Parameters

state

Returns

double

Definition at line 795 of file convenience_state_getters.h.

{ return getAccelerationXYZ(state).y; }

getAccY() [2/2]

template<typename T >

double perception_msgs::object_access::getAccY ( const T &  obj )

inline

Get the y-acceleration of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 805 of file convenience_state_getters.h.

{
  return getAccY(obj.state);
}

getClassWithHighestProbability() [1/2]

ObjectClassification perception_msgs::object_access::getClassWithHighestProbability ( const ObjectState &  state )

inline

Get the classification with highest probability of a given object state.

Parameters

state

Returns

ObjectClassification

Definition at line 818 of file convenience_state_getters.h.

{
  ObjectClassification highest_prob_class;
  auto highest_prob_class_it = std::max_element(
    state.classifications.begin(), state.classifications.end(),
    [](const ObjectClassification & c1, const ObjectClassification & c2) {
      return c1.probability < c2.probability;
    });
  if (highest_prob_class_it == state.classifications.end()) {
    highest_prob_class.probability = -1.0;
    highest_prob_class.type = ObjectClassification::UNKNOWN;
  } else {
    highest_prob_class = *highest_prob_class_it;
  }
  return highest_prob_class;
}

getClassWithHighestProbability() [2/2]

template<typename T >

ObjectClassification perception_msgs::object_access::getClassWithHighestProbability ( const T &  obj )

inline

Get the classification with highest probability of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

ObjectClassification

Definition at line 843 of file convenience_state_getters.h.

{
  return getClassWithHighestProbability(obj.state);
}

getContinuousState() [1/2]

std::vector< double > perception_msgs::object_access::getContinuousState ( const ObjectState &  state )

inline

Get the continuous state for a given object state.

Parameters

state

Returns

std::vector<double>

Definition at line 53 of file convenience_state_getters.h.

{
  sanityCheckContinuousState(state);
  return state.continuous_state;
}

getContinuousState() [2/2]

template<typename T >

std::vector< double > perception_msgs::object_access::getContinuousState ( const T &  obj )

inline

Get the continuous state for a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

std::vector<double>

Definition at line 67 of file convenience_state_getters.h.

{
  return getContinuousState(obj.state);
}

getContinuousStateCovariance() [1/2]

std::vector< double > perception_msgs::object_access::getContinuousStateCovariance ( const ObjectState &  state )

inline

Get the continuous state covariance for a given object state.

Parameters

state

Returns

std::vector<double>

Definition at line 103 of file convenience_state_getters.h.

{
  sanityCheckContinuousState(state);
  return state.continuous_state_covariance;
}

getContinuousStateCovariance() [2/2]

template<typename T >

std::vector< double > perception_msgs::object_access::getContinuousStateCovariance ( const T &  obj )

inline

Get the continuous state covariance for a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

std::vector<double>

Definition at line 117 of file convenience_state_getters.h.

{
  return getContinuousStateCovariance(obj.state);
}

getContinuousStateCovarianceAt() [1/2]

double perception_msgs::object_access::getContinuousStateCovarianceAt ( const ObjectState &  state, const unsigned int  i, const unsigned int  j  )

inline

Get the continuous state covariance entry (i,j) for a given object state.

Parameters

state
i
j

Returns

double

Definition at line 130 of file convenience_state_getters.h.

{
  const int n = getContinuousStateSize(state);
  const std::vector<double> covariance = getContinuousStateCovariance(state);
  return covariance[n * i + j];
}

getContinuousStateCovarianceAt() [2/2]

template<typename T >

double perception_msgs::object_access::getContinuousStateCovarianceAt ( const T &  obj, const unsigned int  i, const unsigned int  j  )

inline

Get the continuous state covariance entry (i,j) for a given template object that contains an object state.

Template Parameters

T

Parameters

obj
i
j

Returns

double

Definition at line 148 of file convenience_state_getters.h.

{
  return getContinuousStateCovarianceAt(obj.state, i, j);
}

getContinuousStateCovarianceDiagonal() [1/2]

std::vector< double > perception_msgs::object_access::getContinuousStateCovarianceDiagonal ( const ObjectState &  state )

inline

Get the continuous state covariance diagonal for a given object state.

Parameters

state

Returns

std::vector<double>

Definition at line 160 of file convenience_state_getters.h.

{
  const int n = getContinuousStateSize(state);
  std::vector<double> diagonal(n);
  for (int i = 0; i < n; i++) diagonal[i] = getContinuousStateCovarianceAt(state, i, i);
  return diagonal;
}

getContinuousStateCovarianceDiagonal() [2/2]

template<typename T >

std::vector< double > perception_msgs::object_access::getContinuousStateCovarianceDiagonal ( const T &  obj )

inline

Get the continuous state covariance diagonal for a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

std::vector<double>

Definition at line 176 of file convenience_state_getters.h.

{
  return getContinuousStateCovarianceDiagonal(obj.state);
}

getDiscreteState() [1/2]

std::vector< long int > perception_msgs::object_access::getDiscreteState ( const ObjectState &  state )

inline

Get the discrete state for a given object state.

Parameters

state

Returns

std::vector<long int>

Definition at line 78 of file convenience_state_getters.h.

{
  sanityCheckContinuousState(state);
  return state.discrete_state;
}

getDiscreteState() [2/2]

template<typename T >

std::vector< long int > perception_msgs::object_access::getDiscreteState ( const T &  obj )

inline

Get the discrete state for a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

std::vector<long int>

Definition at line 92 of file convenience_state_getters.h.

{
  return getDiscreteState(obj.state);
}

getOrientation() [1/2]

gm::Quaternion perception_msgs::object_access::getOrientation ( const ObjectState &  state )

inline

Get the orientation of a given object state.

Parameters

state

Returns

gm::Quaternion

Definition at line 218 of file convenience_state_getters.h.

{
  tf2::Quaternion q;
  double roll{0}, pitch{0}, yaw{0};
  if (hasRoll(state.model_id)) roll = getRoll(state);
  if (hasPitch(state.model_id)) pitch = getPitch(state);
  if (hasYaw(state.model_id)) yaw = getYaw(state);
  q.setRPY(roll, pitch, yaw);
  return tf2::toMsg(q);
}

getOrientation() [2/2]

template<typename T >

gm::Quaternion perception_msgs::object_access::getOrientation ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::Quaternion

Definition at line 237 of file convenience_state_getters.h.

{
  return getOrientation(obj.state);
}

getPitchInDeg() [1/2]

double perception_msgs::object_access::getPitchInDeg ( const ObjectState &  state )

inline

Get the pitch in degree of a given object state.

Parameters

state

Returns

double

Definition at line 500 of file convenience_state_getters.h.

{ return getPitch(state) * 180.0 / M_PI; }

getPitchInDeg() [2/2]

template<typename T >

double perception_msgs::object_access::getPitchInDeg ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 510 of file convenience_state_getters.h.

{
  return getPitchInDeg(obj.state);
}

getPose() [1/2]

gm::Pose perception_msgs::object_access::getPose ( const ObjectState &  state )

inline

Get the pose of a given object state.

Parameters

state

Returns

gm::Pose

Definition at line 248 of file convenience_state_getters.h.

{
  gm::Pose pose;
  pose.position = getPosition(state);
  pose.orientation = getOrientation(state);
  return pose;
}

getPose() [2/2]

template<typename T >

gm::Pose perception_msgs::object_access::getPose ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::Pose

Definition at line 264 of file convenience_state_getters.h.

{
  return getPose(obj.state);
}

getPoseCovariance() [1/2]

std::vector< double > perception_msgs::object_access::getPoseCovariance ( const ObjectState &  state )

inline

Get the pose covariance of a given object state.

Parameters

state

Returns

std::vector<double>

Definition at line 275 of file convenience_state_getters.h.

{
  const int n = 6;
  const int model_id = state.model_id;
  std::vector<double> pose_covariance(n * n, 0.0);
  int ix, jx;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (i == 0 && hasX(model_id))
        ix = indexX(model_id);
      else if (i == 1 && hasY(model_id))
        ix = indexY(model_id);
      else if (i == 2 && hasZ(model_id))
        ix = indexZ(model_id);
      else if (i == 3 && hasRoll(model_id))
        ix = indexRoll(model_id);
      else if (i == 4 && hasPitch(model_id))
        ix = indexPitch(model_id);
      else if (i == 5 && hasYaw(model_id))
        ix = indexYaw(model_id);
      else
        continue;
 
      if (j == 0 && hasX(model_id))
        jx = indexX(model_id);
      else if (j == 1 && hasY(model_id))
        jx = indexY(model_id);
      else if (j == 2 && hasZ(model_id))
        jx = indexZ(model_id);
      else if (j == 3 && hasRoll(model_id))
        jx = indexRoll(model_id);
      else if (j == 4 && hasPitch(model_id))
        jx = indexPitch(model_id);
      else if (j == 5 && hasYaw(model_id))
        jx = indexYaw(model_id);
      else
        continue;
 
      pose_covariance[n * i + j] = getContinuousStateCovarianceAt(state, ix, jx);
    }
  }
  return pose_covariance;
}

getPoseCovariance() [2/2]

template<typename T >

std::vector< double > perception_msgs::object_access::getPoseCovariance ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

std::vector<double>

Definition at line 327 of file convenience_state_getters.h.

{
  return getPoseCovariance(obj.state);
}

getPoseWithCovariance() [1/2]

gm::PoseWithCovariance perception_msgs::object_access::getPoseWithCovariance ( const ObjectState &  state )

inline

Get the pose with covariance of a given object state.

Parameters

state

Returns

gm::PoseWithCovariance

Definition at line 339 of file convenience_state_getters.h.

{
  gm::PoseWithCovariance pose_with_covariance;
  pose_with_covariance.pose = getPose(state);
  std::vector<double> covariance_vector = getPoseCovariance(state);
  gm::PoseWithCovariance::_covariance_type covariance;
  std::copy(covariance_vector.begin(), covariance_vector.end(), covariance.begin());
  pose_with_covariance.covariance = covariance;
  return pose_with_covariance;
}

getPoseWithCovariance() [2/2]

template<typename T >

gm::PoseWithCovariance perception_msgs::object_access::getPoseWithCovariance ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::PoseWithCovariance

Definition at line 358 of file convenience_state_getters.h.

{
  return getPoseWithCovariance(obj.state);
}

getPosition() [1/2]

gm::Point perception_msgs::object_access::getPosition ( const ObjectState &  state )

inline

Get the position of a given object state.

Parameters

state

Returns

gm::Point

Definition at line 190 of file convenience_state_getters.h.

{
  gm::Point position;
  position.x = getX(state);
  position.y = getY(state);
  position.z = getZ(state);
  return position;
}

getPosition() [2/2]

template<typename T >

gm::Point perception_msgs::object_access::getPosition ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::Point

Definition at line 207 of file convenience_state_getters.h.

{
  return getPosition(obj.state);
}

getRollInDeg() [1/2]

double perception_msgs::object_access::getRollInDeg ( const ObjectState &  state )

inline

Get the roll in degree of a given object state.

Parameters

state

Returns

double

Definition at line 479 of file convenience_state_getters.h.

{ return getRoll(state) * 180.0 / M_PI; }

getRollInDeg() [2/2]

template<typename T >

double perception_msgs::object_access::getRollInDeg ( const T &  obj )

inline

Get the roll in degree of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 489 of file convenience_state_getters.h.

{
  return getRollInDeg(obj.state);
}

getVelocity() [1/2]

gm::Vector3 perception_msgs::object_access::getVelocity ( const ObjectState &  state )

inline

Get the velocity of a given object state.

Parameters

state

Returns

gm::Vector3

Definition at line 369 of file convenience_state_getters.h.

{
  gm::Vector3 velocity;
  velocity.x = getVelLon(state);
  velocity.y = getVelLat(state);
  velocity.z = 0.0;
  return velocity;
}

getVelocity() [2/2]

template<typename T >

gm::Vector3 perception_msgs::object_access::getVelocity ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::Vector3

Definition at line 386 of file convenience_state_getters.h.

{
  return getVelocity(obj.state);
}

getVelocityMagnitude() [1/2]

double perception_msgs::object_access::getVelocityMagnitude ( const ObjectState &  state )

inline

Get the velocity magnitude of a given object state.

Parameters

state

Returns

double

Definition at line 397 of file convenience_state_getters.h.

{
  gm::Vector3 vel = getVelocity(state);
  using namespace std;
  return sqrt(pow(vel.x, 2) + pow(vel.y, 2) + pow(vel.z, 2));
}

getVelocityMagnitude() [2/2]

template<typename T >

double perception_msgs::object_access::getVelocityMagnitude ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 412 of file convenience_state_getters.h.

{
  return getVelocityMagnitude(obj.state);
}

getVelocityXYZ() [1/2]

gm::Vector3 perception_msgs::object_access::getVelocityXYZ ( const ObjectState &  state )

inline

Get the velocity XYZ of a given object state.

Parameters

state

Returns

gm::Vector3

Definition at line 607 of file convenience_state_getters.h.

{
  gm::Vector3 vel_lon_lat, vel_xyz;
  vel_lon_lat = getVelocity(state);
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, getYaw(state));
  gm::TransformStamped tf;
  tf.transform.rotation = tf2::toMsg(q);
  tf2::doTransform(vel_lon_lat, vel_xyz, tf);
  return vel_xyz;
}

getVelocityXYZ() [2/2]

template<typename T >

gm::Vector3 perception_msgs::object_access::getVelocityXYZ ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

gm::Vector3

Definition at line 627 of file convenience_state_getters.h.

{
  return getVelocityXYZ(obj.state);
}

getVelocityXYZWithCovariance() [1/2]

gm::PoseWithCovariance perception_msgs::object_access::getVelocityXYZWithCovariance ( const ObjectState &  state )

inline

Get the velocity XYZ with covariance of a object state.

Parameters

state

Returns

gm::PoseWithCovariance

Definition at line 542 of file convenience_state_getters.h.

{
  gm::PoseWithCovariance vel_lon_lat, vel_xyz;
  vel_lon_lat.pose.position.x = getVelLon(state);
  vel_lon_lat.pose.position.y = getVelLat(state);
  vel_lon_lat.pose.position.z = 0.0;
 
  int ix, jx, n = 2;
  auto model_id = state.model_id;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (i == 0 && hasVelLon(model_id))
        ix = indexVelLon(model_id);
      else if (i == 1 && hasVelLat(model_id))
        ix = indexVelLat(model_id);
      else
        continue;
 
      if (j == 0 && hasVelLon(model_id))
        jx = indexVelLon(model_id);
      else if (j == 1 && hasVelLat(model_id))
        jx = indexVelLat(model_id);
      else
        continue;
 
      vel_lon_lat.covariance.at(6 * i + j) = getContinuousStateCovarianceAt(state, ix, jx);
    }
  }
 
  tf2::Quaternion q;
  q.setRPY(0.0, 0.0, getYaw(state));
  gm::TransformStamped tf;
  tf.transform.rotation = tf2::toMsg(q);
 
#ifdef ROS1
  gm::PoseWithCovarianceStamped vel_lon_lat_stamped, vel_xyz_stamped;
  vel_lon_lat_stamped.pose = vel_lon_lat;
  tf2::doTransform(vel_lon_lat_stamped, vel_xyz_stamped, tf);
  vel_xyz = vel_xyz_stamped.pose;
#else
  tf2::doTransform(vel_lon_lat, vel_xyz, tf);
#endif
 
  return vel_xyz;
}

getVelocityXYZWithCovariance() [2/2]

template<typename T >

gm::PoseWithCovariance perception_msgs::object_access::getVelocityXYZWithCovariance ( const T &  obj )

inline

Get the velocity XYZ with covariance of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

gm::PoseWithCovariance

Definition at line 596 of file convenience_state_getters.h.

{
  return getVelocityXYZWithCovariance(obj.state);
}

getVelX() [1/2]

double perception_msgs::object_access::getVelX ( const ObjectState &  state )

inline

Get the x-velocity of a given object state.

Parameters

state

Returns

double

Definition at line 638 of file convenience_state_getters.h.

{ return getVelocityXYZ(state).x; }

getVelX() [2/2]

template<typename T >

double perception_msgs::object_access::getVelX ( const T &  obj )

inline

Get the x-velocity of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 648 of file convenience_state_getters.h.

{
  return getVelX(obj.state);
}

getVelY() [1/2]

double perception_msgs::object_access::getVelY ( const ObjectState &  state )

inline

Get the y-velocity of a given object state.

Parameters

state

Returns

double

Definition at line 659 of file convenience_state_getters.h.

{ return getVelocityXYZ(state).y; }

getVelY() [2/2]

template<typename T >

double perception_msgs::object_access::getVelY ( const T &  obj )

inline

Get the y-velocity of a given template object that contains an object state.

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 669 of file convenience_state_getters.h.

{
  return getVelY(obj.state);
}

getYawInDeg() [1/2]

double perception_msgs::object_access::getYawInDeg ( const ObjectState &  state )

inline

Get the yaw in degree of a given object state.

Parameters

state

Returns

double

Definition at line 521 of file convenience_state_getters.h.

{ return getYaw(state) * 180.0 / M_PI; }

getYawInDeg() [2/2]

template<typename T >

double perception_msgs::object_access::getYawInDeg ( const T &  obj )

inline

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

Template Parameters

T

Parameters

obj

Returns

double

Definition at line 531 of file convenience_state_getters.h.

{
  return getYawInDeg(obj.state);
}