convenience_state_getters.hpp File Reference

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

#include <perception_msgs_utils/checks.hpp>
#include <perception_msgs_utils/covariance_transform.hpp>
#include <perception_msgs_utils/state_getters.hpp>
#include <perception_msgs_utils/state_index.hpp>
#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 full continuous state covariance for a given object state. Empty storage yields INVALID diagonals and zero cross terms without modifying the 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.

Definition in file convenience_state_getters.hpp.

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 414 of file convenience_state_getters.hpp.

                                                           {
  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 430 of file convenience_state_getters.hpp.

                                               {
  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 440 of file convenience_state_getters.hpp.

                                                                 {
  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 454 of file convenience_state_getters.hpp.

                                                     {
  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 711 of file convenience_state_getters.hpp.

                                                              {
  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 730 of file convenience_state_getters.hpp.

                                                  {
  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 654 of file convenience_state_getters.hpp.

                                                                                       {
  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);
  gm::PoseWithCovarianceStamped acc_lon_lat_stamped, acc_xyz_stamped;
  acc_lon_lat_stamped.pose = acc_lon_lat;
  transformPoseWithCovariance(acc_lon_lat_stamped, acc_xyz_stamped, tf);
  acc_xyz = acc_xyz_stamped.pose;
  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 701 of file convenience_state_getters.hpp.

                                                                           {
  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 740 of file convenience_state_getters.hpp.

{ 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 750 of file convenience_state_getters.hpp.

                                    {
  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 760 of file convenience_state_getters.hpp.

{ 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 770 of file convenience_state_getters.hpp.

                                    {
  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 231 of file convenience_state_getters.hpp.

                                                           {
  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 253 of file convenience_state_getters.hpp.

                                                  {
  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 782 of file convenience_state_getters.hpp.

                                                                                     {
  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 804 of file convenience_state_getters.hpp.

                                                                         {
  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 31 of file convenience_state_getters.hpp.

                                                                      {
  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 44 of file convenience_state_getters.hpp.

                                                          {
  return getContinuousState(obj.state);
}

getContinuousStateCovariance() [1/2]

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

inline

Get the full continuous state covariance for a given object state. Empty storage yields INVALID diagonals and zero cross terms without modifying the state.

Parameters

state

Returns

std::vector<double>

Definition at line 78 of file convenience_state_getters.hpp.

                                                                                {
  sanityCheckContinuousStateCovariance(state);
  if (!state.continuous_state_covariance.empty()) return state.continuous_state_covariance;
  const int n = getContinuousStateSize(state.model_id);
  std::vector<double> covariance(n * n, CONTINUOUS_STATE_COVARIANCE_INIT);
  for (int i = 0; i < n; ++i) covariance[n * i + i] = CONTINUOUS_STATE_COVARIANCE_INVALID;
  return 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 95 of file convenience_state_getters.hpp.

                                                                    {
  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 107 of file convenience_state_getters.hpp.

                                                                                                                   {
  const int n = getContinuousStateSize(state);
  if (i >= static_cast<unsigned int>(n) || j >= static_cast<unsigned int>(n))
    throw std::out_of_range("Covariance coordinates out of range");
  sanityCheckContinuousStateCovariance(state);
  const auto& covariance = state.continuous_state_covariance;
  if (covariance.empty()) return i == j ? CONTINUOUS_STATE_COVARIANCE_INVALID : CONTINUOUS_STATE_COVARIANCE_INIT;
  return covariance.at(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 127 of file convenience_state_getters.hpp.

                                                                                                       {
  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 137 of file convenience_state_getters.hpp.

                                                                                        {
  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 152 of file convenience_state_getters.hpp.

                                                                            {
  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 54 of file convenience_state_getters.hpp.

                                                                      {
  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 67 of file convenience_state_getters.hpp.

                                                          {
  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 190 of file convenience_state_getters.hpp.

                                                             {
  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 208 of file convenience_state_getters.hpp.

                                                 {
  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 486 of file convenience_state_getters.hpp.

{ 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 496 of file convenience_state_getters.hpp.

                                          {
  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 218 of file convenience_state_getters.hpp.

                                                {
  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 265 of file convenience_state_getters.hpp.

                                    {
  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.hpp.

                                                                     {
  const int n = 6;
  const int model_id = state.model_id;
  std::vector<double> pose_covariance(n * n, CONTINUOUS_STATE_COVARIANCE_INIT);
  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 326 of file convenience_state_getters.hpp.

                                                         {
  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 337 of file convenience_state_getters.hpp.

                                                                            {
  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 355 of file convenience_state_getters.hpp.

                                                                {
  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 164 of file convenience_state_getters.hpp.

                                                     {
  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 180 of file convenience_state_getters.hpp.

                                         {
  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 466 of file convenience_state_getters.hpp.

{ 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 476 of file convenience_state_getters.hpp.

                                         {
  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 365 of file convenience_state_getters.hpp.

                                                       {
  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 381 of file convenience_state_getters.hpp.

                                           {
  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 391 of file convenience_state_getters.hpp.

                                                             {
  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 405 of file convenience_state_getters.hpp.

                                                 {
  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 585 of file convenience_state_getters.hpp.

                                                          {
  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 604 of file convenience_state_getters.hpp.

                                              {
  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 526 of file convenience_state_getters.hpp.

                                                                                   {
  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);
 
  gm::PoseWithCovarianceStamped vel_lon_lat_stamped, vel_xyz_stamped;
  vel_lon_lat_stamped.pose = vel_lon_lat;
  transformPoseWithCovariance(vel_lon_lat_stamped, vel_xyz_stamped, tf);
  vel_xyz = vel_xyz_stamped.pose;
 
  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 575 of file convenience_state_getters.hpp.

                                                                       {
  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 614 of file convenience_state_getters.hpp.

{ 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 624 of file convenience_state_getters.hpp.

                                    {
  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 634 of file convenience_state_getters.hpp.

{ 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 644 of file convenience_state_getters.hpp.

                                    {
  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 506 of file convenience_state_getters.hpp.

{ 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 516 of file convenience_state_getters.hpp.

                                        {
  return getYawInDeg(obj.state);
}