cl_px4_mr::CpForwardObstacleGuard Class Reference

#include <cp_forward_obstacle_guard.hpp>

Inheritance diagram for cl_px4_mr::CpForwardObstacleGuard:

Collaboration diagram for cl_px4_mr::CpForwardObstacleGuard:

Public Member Functions
	CpForwardObstacleGuard (ForwardObstacleGuardParams params={})
virtual	~CpForwardObstacleGuard ()
void	onInitialize () override
template<typename T >
smacc2::SmaccSignalConnection	onObstacleTooClose (void(T::*callback)(), T *object)
template<typename T >
smacc2::SmaccSignalConnection	onObstacleCleared (void(T::*callback)(), T *object)
void	reset ()
bool	isTooClose () const
float	lastYawRate () const
float	lastMinRange () const
int	lastHits () const
const ForwardObstacleGuardParams &	params () const
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Public Attributes
smacc2::SmaccSignal< void()>	onObstacleTooClose_
smacc2::SmaccSignal< void()>	onObstacleCleared_

Private Member Functions
void	onCloud (const sensor_msgs::msg::PointCloud2 &msg)
void	onAttitude (const px4_msgs::msg::VehicleAttitude::SharedPtr msg)
bool	levelRotation (std::array< float, 9 > &r) const

Private Attributes
ForwardObstacleGuardParams	params_
smacc2::client_core_components::CpTopicSubscriber< sensor_msgs::msg::PointCloud2 > *	subscriber_
rclcpp::Subscription< px4_msgs::msg::VehicleAttitude >::SharedPtr	attitudeSub_
double	lastYaw_ = 0.0
uint64_t	lastYawStampUs_ = 0
std::mutex	attitudeMutex_
std::array< float, 4 >	attitudeQ_ {{1.0f, 0.0f, 0.0f, 0.0f}}
bool	haveAttitude_ = false
std::atomic< float >	yawRate_ {0.0f}
int	gatedClouds_ = 0
float	cosHalfAngle_ = 1.0f
int	triggerStreak_ = 0
int	clearStreak_ = 0
std::atomic< bool >	tooClose_ {false}
std::atomic< float >	lastMinRange_ {std::numeric_limits<float>::infinity()}
std::atomic< int >	lastHits_ {0}

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 85 of file cp_forward_obstacle_guard.hpp.

Constructor & Destructor Documentation

CpForwardObstacleGuard()

cl_px4_mr::CpForwardObstacleGuard::CpForwardObstacleGuard ( ForwardObstacleGuardParams params = {} )

explicit

Definition at line 32 of file cp_forward_obstacle_guard.cpp.

: params_(params), cosHalfAngle_(std::cos(params.halfAngleRad))
{
}

~CpForwardObstacleGuard()

cl_px4_mr::CpForwardObstacleGuard::~CpForwardObstacleGuard ( )

virtual

Definition at line 37 of file cp_forward_obstacle_guard.cpp.

{}

Member Function Documentation

isTooClose()

bool cl_px4_mr::CpForwardObstacleGuard::isTooClose ( ) const

inline

Definition at line 111 of file cp_forward_obstacle_guard.hpp.

{ return tooClose_.load(); }

References tooClose_.

lastHits()

int cl_px4_mr::CpForwardObstacleGuard::lastHits ( ) const

inline

Definition at line 114 of file cp_forward_obstacle_guard.hpp.

{ return lastHits_.load(); }

References lastHits_.

lastMinRange()

float cl_px4_mr::CpForwardObstacleGuard::lastMinRange ( ) const

inline

Definition at line 113 of file cp_forward_obstacle_guard.hpp.

{ return lastMinRange_.load(); }

References lastMinRange_.

lastYawRate()

float cl_px4_mr::CpForwardObstacleGuard::lastYawRate ( ) const

inline

Definition at line 112 of file cp_forward_obstacle_guard.hpp.

{ return yawRate_.load(); }

References yawRate_.

levelRotation()

bool cl_px4_mr::CpForwardObstacleGuard::levelRotation ( std::array< float, 9 > & r ) const

private

Definition at line 103 of file cp_forward_obstacle_guard.cpp.

{
  std::array<float, 4> q;
  {
    std::lock_guard<std::mutex> lock(attitudeMutex_);
    if (!haveAttitude_)
    {
      return false;
    }
    q = attitudeQ_;
  }
  // body FLU -> ENU (px4_ros_com constants), then strip the yaw so the result
  // is the roll/pitch-only rotation into a level, heading-aligned frame
  const tf2::Quaternion kNedEnuQ(0.70710678118654752, 0.70710678118654752, 0.0, 0.0);
  const tf2::Quaternion kFrdFluQ(1.0, 0.0, 0.0, 0.0);
  const tf2::Quaternion qNedFrd(q[1], q[2], q[3], q[0]);
  tf2::Quaternion qEnuFlu = kNedEnuQ * qNedFrd * kFrdFluQ;
  qEnuFlu.normalize();
  const tf2::Matrix3x3 m(qEnuFlu);
  const double yaw = std::atan2(m[1][0], m[0][0]);
  tf2::Matrix3x3 unyaw;
  unyaw.setRPY(0.0, 0.0, -yaw);
  const tf2::Matrix3x3 level = unyaw * m;
  for (int i = 0; i < 3; ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      r[i * 3 + j] = static_cast<float>(level[i][j]);
    }
  }
  return true;
}

References attitudeMutex_, attitudeQ_, and haveAttitude_.

Referenced by onCloud().

Here is the caller graph for this function:

onAttitude()

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

private

Definition at line 78 of file cp_forward_obstacle_guard.cpp.

{
  // yaw rate by finite difference of the NED heading (PX4 does not bridge
  // vehicle_angular_velocity by default); light EMA against quantisation
  const double w = msg->q[0], x = msg->q[1], y = msg->q[2], z = msg->q[3];
  const double yaw = std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
  if (lastYawStampUs_ != 0 && msg->timestamp > lastYawStampUs_)
  {
    const double dt = static_cast<double>(msg->timestamp - lastYawStampUs_) * 1e-6;
    double d = yaw - lastYaw_;
    d = std::atan2(std::sin(d), std::cos(d));
    if (dt > 1e-4 && dt < 0.5)
    {
      const float rate = static_cast<float>(d / dt);
      yawRate_ = 0.5f * yawRate_.load() + 0.5f * rate;
    }
  }
  lastYaw_ = yaw;
  lastYawStampUs_ = msg->timestamp;
 
  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_, haveAttitude_, lastYaw_, lastYawStampUs_, and yawRate_.

Referenced by onInitialize().

Here is the caller graph for this function:

onCloud()

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

private

Definition at line 136 of file cp_forward_obstacle_guard.cpp.

{
  if (params_.maxYawRateRadS > 0.0f && std::fabs(yawRate_.load()) > params_.maxYawRateRadS)
  {
    // turning: the cone is sweeping sideways; keep the current flag, count nothing
    if (++gatedClouds_ == 1)
    {
      RCLCPP_INFO(
        getLogger(), "CpForwardObstacleGuard: yawing at %.2f rad/s - cone gated", yawRate_.load());
    }
    return;
  }
  if (gatedClouds_ > 0)
  {
    RCLCPP_INFO(
      getLogger(), "CpForwardObstacleGuard: cone active again after %d gated clouds", gatedClouds_);
    gatedClouds_ = 0;
    triggerStreak_ = 0;
  }
 
  int hits = 0;
  float minRange = std::numeric_limits<float>::infinity();
 
  std::array<float, 9> r{};
  const bool level = params_.levelFrame && levelRotation(r);
 
  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;  // gz reports no-return as inf (is_dense = false)
    }
    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 (params_.floorCutoffM > 0.0f && z < -params_.floorCutoffM)
    {
      continue;  // the floor, not something in the way
    }
    const float r = std::sqrt(x * x + y * y + z * z);
    if (r < params_.minRangeM || r > params_.clearRangeM)
    {
      continue;
    }
    if (x / r < cosHalfAngle_)
    {
      continue;  // outside the forward cone
    }
    if (r < minRange)
    {
      minRange = r;
    }
    if (r < params_.triggerRangeM)
    {
      ++hits;
    }
  }
 
  lastMinRange_ = minRange;
  lastHits_ = hits;
 
  const bool triggering = hits >= params_.minHits;
  // cleared only when nothing in the cone is below the clear range at all
  const bool clear = !(minRange < params_.clearRangeM);
 
  if (!tooClose_)
  {
    triggerStreak_ = triggering ? triggerStreak_ + 1 : 0;
    if (triggerStreak_ >= params_.triggerClouds)
    {
      tooClose_ = true;
      clearStreak_ = 0;
      RCLCPP_WARN(
        getLogger(), "CpForwardObstacleGuard: OBSTACLE min range %.2f m (%d hits in cone)",
        minRange, hits);
      onObstacleTooClose_();
    }
  }
  else
  {
    clearStreak_ = clear ? clearStreak_ + 1 : 0;
    if (clearStreak_ >= params_.clearClouds)
    {
      tooClose_ = false;
      triggerStreak_ = 0;
      RCLCPP_INFO(getLogger(), "CpForwardObstacleGuard: cleared (min range %.2f m)", minRange);
      onObstacleCleared_();
    }
  }
}

References cl_px4_mr::ForwardObstacleGuardParams::clearClouds, cl_px4_mr::ForwardObstacleGuardParams::clearRangeM, clearStreak_, cosHalfAngle_, cl_px4_mr::ForwardObstacleGuardParams::floorCutoffM, gatedClouds_, smacc2::ISmaccComponent::getLogger(), lastHits_, lastMinRange_, cl_px4_mr::ForwardObstacleGuardParams::levelFrame, levelRotation(), cl_px4_mr::ForwardObstacleGuardParams::maxYawRateRadS, cl_px4_mr::ForwardObstacleGuardParams::minHits, onObstacleCleared_, onObstacleTooClose_, params_, tooClose_, cl_px4_mr::ForwardObstacleGuardParams::triggerClouds, cl_px4_mr::ForwardObstacleGuardParams::triggerRangeM, triggerStreak_, and yawRate_.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_px4_mr::CpForwardObstacleGuard::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 39 of file cp_forward_obstacle_guard.cpp.

{
  this->requiresComponent(subscriber_);
  if (subscriber_ == nullptr)
  {
    RCLCPP_ERROR(
      getLogger(),
      "CpForwardObstacleGuard: no CpTopicSubscriber<PointCloud2> on this client - guard inactive");
    return;
  }
  subscriber_->onMessageReceived(&CpForwardObstacleGuard::onCloud, this);
  if (params_.levelFrame || params_.maxYawRateRadS > 0.0f)
  {
    attitudeSub_ = this->getNode()->create_subscription<px4_msgs::msg::VehicleAttitude>(
      "/fmu/out/vehicle_attitude", rclcpp::SensorDataQoS(),
      std::bind(&CpForwardObstacleGuard::onAttitude, this, std::placeholders::_1));
  }
  RCLCPP_INFO(
    getLogger(),
    "CpForwardObstacleGuard: subscribed - cone %.0f deg, trigger < %.1f m, clear > %.1f m, "
    "%d hits, %d/%d clouds, yaw-rate gate %.2f rad/s, floor cutoff %.1f m, level frame %s",
    params_.halfAngleRad * 180.0 / M_PI, params_.triggerRangeM, params_.clearRangeM,
    params_.minHits, params_.triggerClouds, params_.clearClouds, params_.maxYawRateRadS,
    params_.floorCutoffM, params_.levelFrame ? "on" : "off");
}

References attitudeSub_, cl_px4_mr::ForwardObstacleGuardParams::clearClouds, cl_px4_mr::ForwardObstacleGuardParams::clearRangeM, cl_px4_mr::ForwardObstacleGuardParams::floorCutoffM, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), cl_px4_mr::ForwardObstacleGuardParams::halfAngleRad, cl_px4_mr::ForwardObstacleGuardParams::levelFrame, cl_px4_mr::ForwardObstacleGuardParams::maxYawRateRadS, cl_px4_mr::ForwardObstacleGuardParams::minHits, onAttitude(), onCloud(), smacc2::client_core_components::CpTopicSubscriber< MessageType >::onMessageReceived(), params_, smacc2::ISmaccComponent::requiresComponent(), subscriber_, cl_px4_mr::ForwardObstacleGuardParams::triggerClouds, and cl_px4_mr::ForwardObstacleGuardParams::triggerRangeM.

Here is the call graph for this function:

onObstacleCleared()

template<typename T >

smacc2::SmaccSignalConnection cl_px4_mr::CpForwardObstacleGuard::onObstacleCleared ( void(T::* callback )(), T * object )

inline

Definition at line 100 of file cp_forward_obstacle_guard.hpp.

  {
    return this->getStateMachine()->createSignalConnection(onObstacleCleared_, callback, object);
  }

References smacc2::ISmaccStateMachine::createSignalConnection(), smacc2::ISmaccComponent::getStateMachine(), and onObstacleCleared_.

Referenced by cl_px4_mr::CbObstacleGuard::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

onObstacleTooClose()

template<typename T >

smacc2::SmaccSignalConnection cl_px4_mr::CpForwardObstacleGuard::onObstacleTooClose ( void(T::* callback )(), T * object )

inline

Definition at line 94 of file cp_forward_obstacle_guard.hpp.

  {
    return this->getStateMachine()->createSignalConnection(onObstacleTooClose_, callback, object);
  }

References smacc2::ISmaccStateMachine::createSignalConnection(), smacc2::ISmaccComponent::getStateMachine(), and onObstacleTooClose_.

Referenced by cl_px4_mr::CbObstacleGuard::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

params()

const ForwardObstacleGuardParams & cl_px4_mr::CpForwardObstacleGuard::params ( ) const

inline

Definition at line 115 of file cp_forward_obstacle_guard.hpp.

{ return params_; }

References params_.

reset()

void cl_px4_mr::CpForwardObstacleGuard::reset

(

)

Definition at line 65 of file cp_forward_obstacle_guard.cpp.

{
  const bool was = tooClose_.exchange(false);
  triggerStreak_ = 0;
  clearStreak_ = 0;
  if (was)
  {
    RCLCPP_INFO(
      getLogger(), "CpForwardObstacleGuard: reset (stale flag dropped, last min range %.2f m)",
      lastMinRange_.load());
  }
}

References clearStreak_, smacc2::ISmaccComponent::getLogger(), lastMinRange_, tooClose_, and triggerStreak_.

Referenced by cl_px4_mr::CbObstacleGuard::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

attitudeMutex_

std::mutex cl_px4_mr::CpForwardObstacleGuard::attitudeMutex_

mutableprivate

Definition at line 132 of file cp_forward_obstacle_guard.hpp.

Referenced by levelRotation(), and onAttitude().

attitudeQ_

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

private

Definition at line 133 of file cp_forward_obstacle_guard.hpp.

{{1.0f, 0.0f, 0.0f, 0.0f}};  // PX4 order w, x, y, z

Referenced by levelRotation(), and onAttitude().

attitudeSub_

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

private

Definition at line 129 of file cp_forward_obstacle_guard.hpp.

Referenced by onInitialize().

clearStreak_

int cl_px4_mr::CpForwardObstacleGuard::clearStreak_ = 0

private

Definition at line 139 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud(), and reset().

cosHalfAngle_

float cl_px4_mr::CpForwardObstacleGuard::cosHalfAngle_ = 1.0f

private

Definition at line 137 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud().

gatedClouds_

int cl_px4_mr::CpForwardObstacleGuard::gatedClouds_ = 0

private

Definition at line 136 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud().

haveAttitude_

bool cl_px4_mr::CpForwardObstacleGuard::haveAttitude_ = false

private

Definition at line 134 of file cp_forward_obstacle_guard.hpp.

Referenced by levelRotation(), and onAttitude().

lastHits_

std::atomic<int> cl_px4_mr::CpForwardObstacleGuard::lastHits_ {0}

private

Definition at line 142 of file cp_forward_obstacle_guard.hpp.

{0};

Referenced by lastHits(), and onCloud().

lastMinRange_

std::atomic<float> cl_px4_mr::CpForwardObstacleGuard::lastMinRange_ {std::numeric_limits<float>::infinity()}

private

Definition at line 141 of file cp_forward_obstacle_guard.hpp.

{std::numeric_limits<float>::infinity()};

Referenced by lastMinRange(), onCloud(), and reset().

lastYaw_

double cl_px4_mr::CpForwardObstacleGuard::lastYaw_ = 0.0

private

Definition at line 130 of file cp_forward_obstacle_guard.hpp.

Referenced by onAttitude().

lastYawStampUs_

uint64_t cl_px4_mr::CpForwardObstacleGuard::lastYawStampUs_ = 0

private

Definition at line 131 of file cp_forward_obstacle_guard.hpp.

Referenced by onAttitude().

onObstacleCleared_

smacc2::SmaccSignal<void()> cl_px4_mr::CpForwardObstacleGuard::onObstacleCleared_

Definition at line 118 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud(), and onObstacleCleared().

onObstacleTooClose_

smacc2::SmaccSignal<void()> cl_px4_mr::CpForwardObstacleGuard::onObstacleTooClose_

Definition at line 117 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud(), and onObstacleTooClose().

params_

ForwardObstacleGuardParams cl_px4_mr::CpForwardObstacleGuard::params_

private

Definition at line 126 of file cp_forward_obstacle_guard.hpp.

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

subscriber_

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

private

Initial value:

=
    nullptr

Definition at line 127 of file cp_forward_obstacle_guard.hpp.

Referenced by onInitialize().

tooClose_

std::atomic<bool> cl_px4_mr::CpForwardObstacleGuard::tooClose_ {false}

private

Definition at line 140 of file cp_forward_obstacle_guard.hpp.

{false};

Referenced by isTooClose(), onCloud(), and reset().

triggerStreak_

int cl_px4_mr::CpForwardObstacleGuard::triggerStreak_ = 0

private

Definition at line 138 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud(), and reset().

yawRate_

std::atomic<float> cl_px4_mr::CpForwardObstacleGuard::yawRate_ {0.0f}

private

Definition at line 135 of file cp_forward_obstacle_guard.hpp.

{0.0f};

Referenced by lastYawRate(), onAttitude(), and onCloud().

---

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

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