state_setters.h

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: MIT
 
#pragma once
 
#include <perception_msgs_utils/impl/state_index.h>
#include <perception_msgs_utils/impl/checks.h>
#include <perception_msgs_utils/impl/utils.h>
 
 
namespace perception_msgs {
 
namespace object_access {
 
  inline void setX(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexX(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setX(T& obj, const double val, const bool reset_covariance = true) {
    setX(obj.state, val, reset_covariance);
  }
 
  inline void setY(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexY(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setY(T& obj, const double val, const bool reset_covariance = true) {
    setY(obj.state, val, reset_covariance);
  }
 
  inline void setZ(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexZ(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setZ(T& obj, const double val, const bool reset_covariance = true) {
    setZ(obj.state, val, reset_covariance);
  }
 
  inline void setVelLon(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexVelLon(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setVelLon(T& obj, const double val, const bool reset_covariance = true) {
    setVelLon(obj.state, val, reset_covariance);
  }
 
  inline void setVelLat(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexVelLat(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setVelLat(T& obj, const double val, const bool reset_covariance = true) {
    setVelLat(obj.state, val, reset_covariance);
  }
 
  inline void setAccLon(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexAccLon(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setAccLon(T& obj, const double val, const bool reset_covariance = true) {
    setAccLon(obj.state, val, reset_covariance);
  }
 
  inline void setAccLat(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexAccLat(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setAccLat(T& obj, const double val, const bool reset_covariance = true) {
    setAccLat(obj.state, val, reset_covariance);
  }
 
  inline void setRoll(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    double capped_val = val;
    capped_val -= 2.0 * M_PI * std::floor((capped_val + M_PI) * (M_1_PI * 0.5));
    if (capped_val <= -M_PI) capped_val += 2 * M_PI;
    const int idx = indexRoll(state.model_id);
    state.continuous_state[idx] = capped_val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setRoll(T& obj, const double val, const bool reset_covariance = true) {
    setRoll(obj.state, val, reset_covariance);
  }
 
  inline void setRollRate(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexRollRate(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setRollRate(T& obj, const double val, const bool reset_covariance = true) {
    setRollRate(obj.state, val, reset_covariance);
  }
 
  inline void setPitch(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    double capped_val = val;
    capped_val -= 2.0 * M_PI * std::floor((capped_val + M_PI) * (M_1_PI * 0.5));
    if (capped_val <= -M_PI) capped_val += 2 * M_PI;
    const int idx = indexPitch(state.model_id);
    state.continuous_state[idx] = capped_val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setPitch(T& obj, const double val, const bool reset_covariance = true) {
    setPitch(obj.state, val, reset_covariance);
  }
 
  inline void setPitchRate(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexPitchRate(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setPitchRate(T& obj, const double val, const bool reset_covariance = true) {
    setPitchRate(obj.state, val, reset_covariance);
  }
 
  inline void setYaw(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    double capped_val = val;
    capped_val -= 2.0 * M_PI * std::floor((capped_val + M_PI) * (M_1_PI * 0.5));
    if (capped_val <= -M_PI) capped_val += 2 * M_PI;
    const int idx = indexYaw(state.model_id);
    state.continuous_state[idx] = capped_val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setYaw(T& obj, const double val, const bool reset_covariance = true) {
    setYaw(obj.state, val, reset_covariance);
  }
 
  inline void setYawRate(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexYawRate(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setYawRate(T& obj, const double val, const bool reset_covariance = true) {
    setYawRate(obj.state, val, reset_covariance);
  }
 
  inline void setSteeringAngleAck(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexSteeringAngleAck(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setSteeringAngleAck(T& obj, const double val, const bool reset_covariance = true) {
    setSteeringAngleAck(obj.state, val, reset_covariance);
  }
 
  inline void setSteeringAngleRateAck(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexSteeringAngleRateAck(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setSteeringAngleRateAck(T& obj, const double val, const bool reset_covariance = true) {
    setSteeringAngleRateAck(obj.state, val, reset_covariance);
  }
 
  inline void setSteeringAngleFront(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexSteeringAngleFront(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setSteeringAngleFront(T& obj, const double val, const bool reset_covariance = true) {
    setSteeringAngleFront(obj.state, val, reset_covariance);
  }
 
  inline void setSteeringAngleRear(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexSteeringAngleRear(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setSteeringAngleRear(T& obj, const double val, const bool reset_covariance = true) {
    setSteeringAngleRear(obj.state, val, reset_covariance);
  }
  
  inline void setWidth(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexWidth(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setWidth(T& obj, const double val, const bool reset_covariance = true) {
    setWidth(obj.state, val, reset_covariance);
  }
 
  inline void setLength(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexLength(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setLength(T& obj, const double val, const bool reset_covariance = true) {
    setLength(obj.state, val, reset_covariance);
  }
 
  inline void setHeight(ObjectState& state, const double val, const bool reset_covariance = true) {
    sanityCheckContinuousState(state);
    const int idx = indexHeight(state.model_id);
    state.continuous_state[idx] = val;
    if (reset_covariance) setContinuousStateCovarianceToUnknownAt(state, idx, idx);
  }
 
  template <typename T>
  inline void setHeight(T& obj, const double val, const bool reset_covariance = true) {
    setHeight(obj.state, val, reset_covariance);
  }
 
  inline void setStandstill(ObjectState& state, const bool val) {
    sanityCheckDiscreteState(state);
    const int idx = indexStandstill(state.model_id);
    state.discrete_state[idx] = val;
  }
 
  template <typename T>
  inline void setStandstill(T& obj, const bool val) {
    setStandstill(obj.state, val);
  }
 
  inline void setTurnIndicator(ObjectState& state, const uint8_t val) {
    sanityCheckDiscreteState(state);
    const int idx = indexTurnIndicator(state.model_id);
    state.discrete_state[idx] = val;
  }
 
  template <typename T>
  inline void setTurnIndicator(T& obj, const uint8_t val) {
    setTurnIndicator(obj.state, val);
  }
 
  inline void setBrakeLight(ObjectState& state, const uint8_t val) {
    sanityCheckDiscreteState(state);
    const int idx = indexBrakeLight(state.model_id);
    state.discrete_state[idx] = val;
  }
 
  template <typename T>
  inline void setBrakeLight(T& obj, const uint8_t val) {
    setBrakeLight(obj.state, val);
  }
 
  inline void setReverseLight(ObjectState& state, const uint8_t val) {
    sanityCheckDiscreteState(state);
    const int idx = indexReverseLight(state.model_id);
    state.discrete_state[idx] = val;
  }
 
  template <typename T>
  inline void setReverseLight(T& obj, const uint8_t val) {
    setReverseLight(obj.state, val);
  }
 
  inline void setTrafficLightState(ObjectState& state, const uint8_t val) {
    sanityCheckDiscreteState(state);
    const int idx = indexTrafficLightState(state.model_id);
    state.discrete_state[idx] = val;
  }
 
  template <typename T>
  inline void setTrafficLightState(T& obj, const uint8_t val) {
    setTrafficLightState(obj.state, val);
  }
 
  inline void setTrafficLightType(ObjectState& state, const uint8_t val) {
    sanityCheckDiscreteState(state);
    const int idx = indexTrafficLightType(state.model_id);
    state.discrete_state[idx] = val;
  }
 
  template <typename T>
  inline void setTrafficLightType(T& obj, const uint8_t val) {
    setTrafficLightType(obj.state, val);
  }
 
} // namespace object_access
 
} // namespace perception_msgs