cl_px4_mr::CpTunnelCentering Class Reference

#include <cp_tunnel_centering.hpp>

Inheritance diagram for cl_px4_mr::CpTunnelCentering:

Collaboration diagram for cl_px4_mr::CpTunnelCentering:

Public Member Functions
	CpTunnelCentering (TunnelCenteringParams params={})
virtual	~CpTunnelCentering ()
void	onInitialize () override
float	lateralOffsetM () const
float	verticalOffsetM () const
bool	valid () const
float	leftWallM () const
float	rightWallM () const
float	floorM () const
float	ceilingM () const
const TunnelCenteringParams &	params () const
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Private Member Functions
void	onCloud (const sensor_msgs::msg::PointCloud2 &msg)
void	onAttitude (const px4_msgs::msg::VehicleAttitude::SharedPtr msg)

Private Attributes
TunnelCenteringParams	params_
smacc2::client_core_components::CpTopicSubscriber< sensor_msgs::msg::PointCloud2 > *	subscriber_
rclcpp::Subscription< px4_msgs::msg::VehicleAttitude >::SharedPtr	attitudeSub_
std::mutex	attitudeMutex_
std::array< float, 4 >	attitudeQ_ {{1.0f, 0.0f, 0.0f, 0.0f}}
bool	haveAttitude_ = false
std::atomic< float >	lateral_ {0.0f}
std::atomic< float >	vertical_ {0.0f}
std::atomic< float >	left_ {0.0f}
std::atomic< float >	right_ {0.0f}
std::atomic< float >	floor_ {0.0f}
std::atomic< float >	ceiling_ {0.0f}
std::atomic< int64_t >	lastCloudNs_ {0}
std::chrono::steady_clock::time_point	lastLog_

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)
template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)
rclcpp::Node::SharedPtr	getNode ()
rclcpp::Logger	getLogger () const
ISmaccStateMachine *	getStateMachine ()
Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 90 of file cp_tunnel_centering.hpp.

Constructor & Destructor Documentation

CpTunnelCentering()

cl_px4_mr::CpTunnelCentering::CpTunnelCentering ( TunnelCenteringParams params = {} )

explicit

Definition at line 57 of file cp_tunnel_centering.cpp.

: params_(params) {}

~CpTunnelCentering()

cl_px4_mr::CpTunnelCentering::~CpTunnelCentering ( )

virtual

Definition at line 59 of file cp_tunnel_centering.cpp.

{}

Member Function Documentation

ceilingM()

float cl_px4_mr::CpTunnelCentering::ceilingM ( ) const

inline

Definition at line 108 of file cp_tunnel_centering.hpp.

{ return ceiling_.load(); }

References ceiling_.

floorM()

float cl_px4_mr::CpTunnelCentering::floorM ( ) const

inline

Definition at line 107 of file cp_tunnel_centering.hpp.

{ return floor_.load(); }

References floor_.

lateralOffsetM()

float cl_px4_mr::CpTunnelCentering::lateralOffsetM ( ) const

inline

Definition at line 99 of file cp_tunnel_centering.hpp.

{ return lateral_.load(); }

References lateral_.

Referenced by cl_px4_mr::CbPx4PathFollowerBase::applyCentering().

Here is the caller graph for this function:

leftWallM()

float cl_px4_mr::CpTunnelCentering::leftWallM ( ) const

inline

Definition at line 105 of file cp_tunnel_centering.hpp.

{ return left_.load(); }

References left_.

onAttitude()

void cl_px4_mr::CpTunnelCentering::onAttitude ( const px4_msgs::msg::VehicleAttitude::SharedPtr msg )

private

Definition at line 94 of file cp_tunnel_centering.cpp.

{
  std::lock_guard<std::mutex> lock(attitudeMutex_);
  attitudeQ_ = {msg->q[0], msg->q[1], msg->q[2], msg->q[3]};
  haveAttitude_ = true;
}

References attitudeMutex_, attitudeQ_, and haveAttitude_.

Referenced by onInitialize().

Here is the caller graph for this function:

onCloud()

void cl_px4_mr::CpTunnelCentering::onCloud ( const sensor_msgs::msg::PointCloud2 & msg )

private

Definition at line 101 of file cp_tunnel_centering.cpp.

{
  std::array<float, 9> r{};
  bool level = false;
  if (params_.levelFrame)
  {
    std::array<float, 4> q;
    {
      std::lock_guard<std::mutex> lock(attitudeMutex_);
      level = haveAttitude_;
      q = attitudeQ_;
    }
    if (level)
    {
      levelRotationFromPx4Quaternion(q, r);
    }
  }
 
  std::vector<float> leftY, rightY, floorZ, ceilZ;
  leftY.reserve(512);
  rightY.reserve(512);
  floorZ.reserve(512);
  ceilZ.reserve(512);
 
  const float minRangeSq = params_.minRangeM * params_.minRangeM;
  sensor_msgs::PointCloud2ConstIterator<float> ix(msg, "x");
  sensor_msgs::PointCloud2ConstIterator<float> iy(msg, "y");
  sensor_msgs::PointCloud2ConstIterator<float> iz(msg, "z");
  for (; ix != ix.end(); ++ix, ++iy, ++iz)
  {
    float x = *ix;
    float y = *iy;
    float z = *iz;
    if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z))
    {
      continue;
    }
    if (x * x + y * y + z * z < minRangeSq)
    {
      continue;  // own airframe
    }
    if (level)
    {
      const float lx = r[0] * x + r[1] * y + r[2] * z;
      const float ly = r[3] * x + r[4] * y + r[5] * z;
      const float lz = r[6] * x + r[7] * y + r[8] * z;
      x = lx;
      y = ly;
      z = lz;
    }
    if (x < -params_.floorLookBackM || x > params_.lookAheadMaxM)
    {
      continue;
    }
    const bool ahead = x >= params_.lookAheadMinM;
    // walls: the band at flight height, in the slab ahead
    if (
      ahead && std::fabs(z) <= params_.wallBandHalfHeightM &&
      std::fabs(y) <= params_.lateralSearchM)
    {
      if (y > 0.0f)
      {
        leftY.push_back(y);
      }
      else if (y < 0.0f)
      {
        rightY.push_back(-y);
      }
    }
    // floor / ceiling: the column along the heading, EXCLUDING the wall band -
    // rock beside the vehicle at sensor height is a wall (lateral channel), not
    // a floor 0.1 m below it (run 22: an arch wall read as "floor 0.1" forced a
    // 1 m climb into the ceiling)
    if (
      std::fabs(y) <= params_.columnHalfWidthM && std::fabs(z) > params_.wallBandHalfHeightM &&
      std::fabs(z) <= params_.verticalSearchM)
    {
      if (z < 0.0f)
      {
        floorZ.push_back(-z);
      }
      else
      {
        ceilZ.push_back(z);
      }
    }
  }
 
  // nearest surfaces, robust to a few stray returns
  const size_t kw = static_cast<size_t>(std::max(params_.minWallPoints, 1));
  const size_t kf = static_cast<size_t>(std::max(params_.minFloorPoints, 1));
  const float left = kthSmallest(leftY, kw);
  const float right = kthSmallest(rightY, kw);
  const float floorD = kthSmallest(floorZ, kf);
  const float ceilD = kthSmallest(ceilZ, kf);
 
  // lateral: toward the midpoint between the walls; needs both, fades when far
  float lateral = 0.0f;
  if (std::isfinite(left) && std::isfinite(right))
  {
    lateral = 0.5f * (left - right);  // > 0: more room on the left -> move left
    const float nearer = std::min(left, right);
    if (nearer > params_.wallFadeM)
    {
      lateral = 0.0f;
    }
    else if (nearer > 0.5f * params_.wallFadeM)
    {
      lateral *= (params_.wallFadeM - nearer) / (0.5f * params_.wallFadeM);
    }
  }
  lateral = std::clamp(lateral, -params_.maxLateralM, params_.maxLateralM);
 
  // vertical: hold the floor clearance, but stay below the ceiling
  float vertical = 0.0f;
  if (std::isfinite(floorD))
  {
    vertical = params_.floorClearanceM - floorD;  // > 0: climb
  }
  if (std::isfinite(ceilD))
  {
    vertical = std::min(vertical, ceilD - params_.ceilingClearanceM);
  }
  if (std::isfinite(floorD))
  {
    // never below the minimum floor clearance, even when the overhead reading
    // (possibly rising terrain ahead) asks for it
    vertical = std::max(vertical, params_.minFloorClearanceM - floorD);
  }
  vertical = std::clamp(vertical, -params_.maxVerticalM, params_.maxVerticalM);
 
  const float a = std::clamp(params_.smoothing, 0.0f, 0.95f);
  const bool first = lastCloudNs_.load() == 0;
  lateral_ = first ? lateral : a * lateral_.load() + (1.0f - a) * lateral;
  vertical_ = first ? vertical : a * vertical_.load() + (1.0f - a) * vertical;
  left_ = left;
  right_ = right;
  floor_ = floorD;
  ceiling_ = ceilD;
  lastCloudNs_ = nowNs();
 
  const auto now = std::chrono::steady_clock::now();
  if (std::chrono::duration<double>(now - lastLog_).count() >= 2.0)
  {
    lastLog_ = now;
    RCLCPP_INFO(
      getLogger(),
      "CpTunnelCentering: walls L %.1f R %.1f, floor %.1f, ceiling %.1f m -> lateral %+.2f, "
      "vertical %+.2f m",
      left, right, floorD, ceilD, lateral_.load(), vertical_.load());
  }
}

References attitudeMutex_, attitudeQ_, ceiling_, cl_px4_mr::TunnelCenteringParams::ceilingClearanceM, cl_px4_mr::TunnelCenteringParams::columnHalfWidthM, floor_, cl_px4_mr::TunnelCenteringParams::floorClearanceM, smacc2::ISmaccComponent::getLogger(), haveAttitude_, lastCloudNs_, lastLog_, lateral_, cl_px4_mr::TunnelCenteringParams::lateralSearchM, left_, cl_px4_mr::TunnelCenteringParams::levelFrame, cl_px4_mr::levelRotationFromPx4Quaternion(), cl_px4_mr::TunnelCenteringParams::lookAheadMaxM, cl_px4_mr::TunnelCenteringParams::lookAheadMinM, cl_px4_mr::TunnelCenteringParams::maxLateralM, cl_px4_mr::TunnelCenteringParams::maxVerticalM, cl_px4_mr::TunnelCenteringParams::minFloorClearanceM, cl_px4_mr::TunnelCenteringParams::minFloorPoints, cl_px4_mr::TunnelCenteringParams::minRangeM, cl_px4_mr::TunnelCenteringParams::minWallPoints, params_, right_, cl_px4_mr::TunnelCenteringParams::smoothing, vertical_, cl_px4_mr::TunnelCenteringParams::verticalSearchM, cl_px4_mr::TunnelCenteringParams::wallBandHalfHeightM, and cl_px4_mr::TunnelCenteringParams::wallFadeM.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_px4_mr::CpTunnelCentering::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 61 of file cp_tunnel_centering.cpp.

{
  this->requiresComponent(subscriber_);
  if (subscriber_ == nullptr)
  {
    RCLCPP_ERROR(
      getLogger(),
      "CpTunnelCentering: no CpTopicSubscriber<PointCloud2> on this client - centering inactive");
    return;
  }
  subscriber_->onMessageReceived(&CpTunnelCentering::onCloud, this);
  if (params_.levelFrame)
  {
    attitudeSub_ = this->getNode()->create_subscription<px4_msgs::msg::VehicleAttitude>(
      "/fmu/out/vehicle_attitude", rclcpp::SensorDataQoS(),
      std::bind(&CpTunnelCentering::onAttitude, this, std::placeholders::_1));
  }
  lastLog_ = std::chrono::steady_clock::now();
  RCLCPP_INFO(
    getLogger(),
    "CpTunnelCentering: subscribed - slab %.1f..%.1f m ahead, floor clearance %.2f m (min %.1f), "
    "ceiling clearance %.1f m, max lateral %.1f m, max vertical %.1f m, wall fade %.1f m",
    params_.lookAheadMinM, params_.lookAheadMaxM, params_.floorClearanceM,
    params_.minFloorClearanceM, params_.ceilingClearanceM, params_.maxLateralM,
    params_.maxVerticalM, params_.wallFadeM);
}

References attitudeSub_, cl_px4_mr::TunnelCenteringParams::ceilingClearanceM, cl_px4_mr::TunnelCenteringParams::floorClearanceM, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), lastLog_, cl_px4_mr::TunnelCenteringParams::levelFrame, cl_px4_mr::TunnelCenteringParams::lookAheadMaxM, cl_px4_mr::TunnelCenteringParams::lookAheadMinM, cl_px4_mr::TunnelCenteringParams::maxLateralM, cl_px4_mr::TunnelCenteringParams::maxVerticalM, cl_px4_mr::TunnelCenteringParams::minFloorClearanceM, onAttitude(), onCloud(), smacc2::client_core_components::CpTopicSubscriber< MessageType >::onMessageReceived(), params_, smacc2::ISmaccComponent::requiresComponent(), subscriber_, and cl_px4_mr::TunnelCenteringParams::wallFadeM.

Here is the call graph for this function:

params()

const TunnelCenteringParams & cl_px4_mr::CpTunnelCentering::params ( ) const

inline

Definition at line 110 of file cp_tunnel_centering.hpp.

{ return params_; }

References params_.

rightWallM()

float cl_px4_mr::CpTunnelCentering::rightWallM ( ) const

inline

Definition at line 106 of file cp_tunnel_centering.hpp.

{ return right_.load(); }

References right_.

valid()

bool cl_px4_mr::CpTunnelCentering::valid

(

)

const

Definition at line 88 of file cp_tunnel_centering.cpp.

{
  const int64_t last = lastCloudNs_.load();
  return last > 0 && (nowNs() - last) < 1000000000LL;
}

References lastCloudNs_.

Referenced by cl_px4_mr::CbPx4PathFollowerBase::applyCentering().

Here is the caller graph for this function:

verticalOffsetM()

float cl_px4_mr::CpTunnelCentering::verticalOffsetM ( ) const

inline

Definition at line 100 of file cp_tunnel_centering.hpp.

{ return vertical_.load(); }

References vertical_.

Referenced by cl_px4_mr::CbPx4PathFollowerBase::applyCentering().

Here is the caller graph for this function:

Member Data Documentation

attitudeMutex_

std::mutex cl_px4_mr::CpTunnelCentering::attitudeMutex_

mutableprivate

Definition at line 120 of file cp_tunnel_centering.hpp.

Referenced by onAttitude(), and onCloud().

attitudeQ_

std::array<float, 4> cl_px4_mr::CpTunnelCentering::attitudeQ_ {{1.0f, 0.0f, 0.0f, 0.0f}}

private

Definition at line 121 of file cp_tunnel_centering.hpp.

{{1.0f, 0.0f, 0.0f, 0.0f}};

Referenced by onAttitude(), and onCloud().

attitudeSub_

rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr cl_px4_mr::CpTunnelCentering::attitudeSub_

private

Definition at line 119 of file cp_tunnel_centering.hpp.

Referenced by onInitialize().

ceiling_

std::atomic<float> cl_px4_mr::CpTunnelCentering::ceiling_ {0.0f}

private

Definition at line 129 of file cp_tunnel_centering.hpp.

{0.0f};

Referenced by ceilingM(), and onCloud().

floor_

std::atomic<float> cl_px4_mr::CpTunnelCentering::floor_ {0.0f}

private

Definition at line 128 of file cp_tunnel_centering.hpp.

{0.0f};

Referenced by floorM(), and onCloud().

haveAttitude_

bool cl_px4_mr::CpTunnelCentering::haveAttitude_ = false

private

Definition at line 122 of file cp_tunnel_centering.hpp.

Referenced by onAttitude(), and onCloud().

lastCloudNs_

std::atomic<int64_t> cl_px4_mr::CpTunnelCentering::lastCloudNs_ {0}

private

Definition at line 130 of file cp_tunnel_centering.hpp.

{0};

Referenced by onCloud(), and valid().

lastLog_

std::chrono::steady_clock::time_point cl_px4_mr::CpTunnelCentering::lastLog_

private

Definition at line 131 of file cp_tunnel_centering.hpp.

Referenced by onCloud(), and onInitialize().

lateral_

std::atomic<float> cl_px4_mr::CpTunnelCentering::lateral_ {0.0f}

private

Definition at line 124 of file cp_tunnel_centering.hpp.

{0.0f};

Referenced by lateralOffsetM(), and onCloud().

left_

std::atomic<float> cl_px4_mr::CpTunnelCentering::left_ {0.0f}

private

Definition at line 126 of file cp_tunnel_centering.hpp.

{0.0f};

Referenced by leftWallM(), and onCloud().

params_

TunnelCenteringParams cl_px4_mr::CpTunnelCentering::params_

private

Definition at line 116 of file cp_tunnel_centering.hpp.

Referenced by onCloud(), onInitialize(), and params().

right_

std::atomic<float> cl_px4_mr::CpTunnelCentering::right_ {0.0f}

private

Definition at line 127 of file cp_tunnel_centering.hpp.

{0.0f};

Referenced by onCloud(), and rightWallM().

subscriber_

smacc2::client_core_components::CpTopicSubscriber<sensor_msgs::msg::PointCloud2>* cl_px4_mr::CpTunnelCentering::subscriber_

private

Initial value:

=
    nullptr

Definition at line 117 of file cp_tunnel_centering.hpp.

Referenced by onInitialize().

vertical_

std::atomic<float> cl_px4_mr::CpTunnelCentering::vertical_ {0.0f}

private

Definition at line 125 of file cp_tunnel_centering.hpp.

{0.0f};

Referenced by onCloud(), and verticalOffsetM().

---

The documentation for this class was generated from the following files:

• smacc2_client_library/cl_px4_mr/include/cl_px4_mr/components/cp_tunnel_centering.hpp
• smacc2_client_library/cl_px4_mr/src/cl_px4_mr/components/cp_tunnel_centering.cpp