Program Listing for File sensing_subsystem.hpp

↰ Return to documentation for file (cosm/subsystem/sensing_subsystem.hpp)

#pragma once

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include "rcppsw/math/vector2.hpp"
#include "rcppsw/math/vector3.hpp"
#include "rcppsw/types/timestep.hpp"

#include "cosm/hal/subsystem/sensing_subsystem.hpp"

/*******************************************************************************
 * Namespaces
 ******************************************************************************/
namespace cosm::subsystem {

/*******************************************************************************
 * Class Definitions
 ******************************************************************************/
class sensing_subsystem final : public chsubsystem::sensing_subsystem {
 public:
  explicit sensing_subsystem(sensor_map&& sensors)
      : chsubsystem::sensing_subsystem(std::move(sensors)) {}

  virtual ~sensing_subsystem(void) = default;

  const rtypes::timestep& tick(void) const { return m_tick; }

  const rmath::radians& azimuth(void) const { return m_azimuth; }

  const rmath::radians& zenith(void) const { return m_zenith; }

  const rmath::vector2d& rpos2D(void) const { return m_rpos2D; }

  const rmath::vector2z& dpos2D(void) const { return m_dpos2D; }

  const rmath::vector3d& rpos3D(void) const { return m_rpos3D; }

  const rmath::vector3z& dpos3D(void) const { return m_dpos3D; }

  void update(const rtypes::timestep& t, const rtypes::discretize_ratio& ratio) {
    /* update real position info */
    update(t);

    /* update discrete position info */
    auto odom = odometry()->reading();
    m_dpos2D = rmath::dvec2zvec(m_rpos2D, ratio.v());
    m_dpos3D = rmath::dvec2zvec(m_rpos3D, ratio.v());
  }

  void update(const rtypes::timestep& t) {
    m_tick = t;
    auto odom = odometry()->reading();

    /* update 2D position info */
    m_prev_rpos2D = m_rpos2D;
    m_rpos2D = odom.pose.position.project_on_xy();

    /* update 3D position info */
    m_prev_rpos3D = m_rpos3D;
    m_rpos3D = odom.pose.position;
    m_azimuth = odom.pose.orientation.z();
    m_zenith = odom.pose.orientation.y();
  }

  rmath::vector2d tick_travel2D(void) const { return m_rpos2D - m_prev_rpos2D; }

  rmath::vector3d tick_travel3D(void) const { return m_rpos3D - m_prev_rpos3D; }

 private:
  /* clang-format off */
  rtypes::timestep              m_tick{0};
  rmath::vector3d               m_rpos3D{};
  rmath::vector3d               m_prev_rpos3D{};
  rmath::vector3z               m_dpos3D{};

  rmath::vector2d               m_rpos2D{};
  rmath::vector2d               m_prev_rpos2D{};
  rmath::vector2z               m_dpos2D{};

  rmath::radians                m_azimuth{};
  rmath::radians                m_zenith{};
  /* clang-format off */
};

} /* namespace cosm::subsystem */