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.
gm::Point	perception_msgs::object_access::getCenterPosition (const ObjectState &state)
	Get the object's geometric center position.
template<typename T >
gm::Point	perception_msgs::object_access::getCenterPosition (const T &object)
	Get the object's geometric center position.
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 425 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 441 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 451 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 730 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 749 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 669 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 720 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 759 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 769 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 779 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 789 of file convenience_state_getters.h.

                                    {
  return getAccY(obj.state);
}

getCenterPosition() [1/2]

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

inline

Get the object's geometric center position.

Parameters

state

The state to get the center position from

Returns

gm::Point The position of the object's geometric center

Definition at line 242 of file convenience_state_getters.h.

                                                           {
  gm::Point position = getPosition(state);
  const auto orientation = getOrientation(state);
  const gm::Vector3 offset_to_center = state.reference_point.translation_to_geometric_center;
  tf2::Quaternion q;
  tf2::fromMsg(orientation, q);
  const tf2::Vector3 offset_to_center_tf2(offset_to_center.x, offset_to_center.y, offset_to_center.z);
  const tf2::Vector3 rotated_offset_to_center = tf2::quatRotate(q, offset_to_center_tf2);
  position.x += rotated_offset_to_center.x();
  position.y += rotated_offset_to_center.y();
  position.z += rotated_offset_to_center.z();
  return position;
}

getCenterPosition() [2/2]

template<typename T >

gm::Point perception_msgs::object_access::getCenterPosition ( const T &  object )

inline

Get the object's geometric center position.

Template Parameters

T

Parameters

object

The object to get the center position from

Returns

gm::Position The position of the object's geometric center

Definition at line 264 of file convenience_state_getters.h.

                                                  {
  return getCenterPosition(object.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 801 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 823 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 51 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 64 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 97 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 110 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 122 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 138 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 148 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 163 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 74 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 87 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 201 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 219 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 497 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 507 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 229 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 276 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 286 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 337 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 348 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 366 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 175 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 191 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 477 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 487 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 376 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 392 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 402 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 416 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 600 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 619 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 537 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 590 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 629 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 639 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 649 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 659 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 517 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 527 of file convenience_state_getters.h.

                                        {
  return getYawInDeg(obj.state);
}