#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 89 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.

37{}

Member Function Documentation

◆ isTooClose()

bool cl_px4_mr::CpForwardObstacleGuard::isTooClose ( ) const

inline

Definition at line 115 of file cp_forward_obstacle_guard.hpp.

115{ return tooClose_.load(); }

cl_px4_mr::CpForwardObstacleGuard::tooClose_

std::atomic< bool > tooClose_

Definition cp_forward_obstacle_guard.hpp:144

References tooClose_.

◆ lastHits()

int cl_px4_mr::CpForwardObstacleGuard::lastHits ( ) const

inline

Definition at line 118 of file cp_forward_obstacle_guard.hpp.

118{ return lastHits_.load(); }

cl_px4_mr::CpForwardObstacleGuard::lastHits_

std::atomic< int > lastHits_

Definition cp_forward_obstacle_guard.hpp:146

References lastHits_.

◆ lastMinRange()

float cl_px4_mr::CpForwardObstacleGuard::lastMinRange ( ) const

inline

Definition at line 117 of file cp_forward_obstacle_guard.hpp.

117{ return lastMinRange_.load(); }

cl_px4_mr::CpForwardObstacleGuard::lastMinRange_

std::atomic< float > lastMinRange_

Definition cp_forward_obstacle_guard.hpp:145

References lastMinRange_.

◆ lastYawRate()

float cl_px4_mr::CpForwardObstacleGuard::lastYawRate ( ) const

inline

Definition at line 116 of file cp_forward_obstacle_guard.hpp.

116{ return yawRate_.load(); }

cl_px4_mr::CpForwardObstacleGuard::yawRate_

std::atomic< float > yawRate_

Definition cp_forward_obstacle_guard.hpp:139

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 (params_.opticalFrameInput)
    {
      // optical (z forward, x right, y down) -> sensor (x forward, y left, z up)
      const float ox = x, oy = y, oz = z;
      x = oz;
      y = -ox;
      z = -oy;
    }
    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_();
    }
  }
}

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");
}

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 104 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 98 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 119 of file cp_forward_obstacle_guard.hpp.

119{ 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 136 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 137 of file cp_forward_obstacle_guard.hpp.

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

Referenced by onInitialize().

◆ clearStreak_

int cl_px4_mr::CpForwardObstacleGuard::clearStreak_ = 0

private

Definition at line 143 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 141 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud().

◆ gatedClouds_

int cl_px4_mr::CpForwardObstacleGuard::gatedClouds_ = 0

private

Definition at line 140 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud().

◆ haveAttitude_

bool cl_px4_mr::CpForwardObstacleGuard::haveAttitude_ = false

private

Definition at line 138 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 146 of file cp_forward_obstacle_guard.hpp.

146{0};

Referenced by lastHits(), and onCloud().

◆ lastMinRange_

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

private

Definition at line 145 of file cp_forward_obstacle_guard.hpp.

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

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

◆ lastYaw_

double cl_px4_mr::CpForwardObstacleGuard::lastYaw_ = 0.0

private

Definition at line 134 of file cp_forward_obstacle_guard.hpp.

Referenced by onAttitude().

◆ lastYawStampUs_

uint64_t cl_px4_mr::CpForwardObstacleGuard::lastYawStampUs_ = 0

private

Definition at line 135 of file cp_forward_obstacle_guard.hpp.

Referenced by onAttitude().

◆ onObstacleCleared_

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

Definition at line 122 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud(), and onObstacleCleared().

◆ onObstacleTooClose_

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

Definition at line 121 of file cp_forward_obstacle_guard.hpp.

Referenced by onCloud(), and onObstacleTooClose().

◆ params_

ForwardObstacleGuardParams cl_px4_mr::CpForwardObstacleGuard::params_

private

Definition at line 130 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 131 of file cp_forward_obstacle_guard.hpp.

Referenced by onInitialize().

◆ tooClose_

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

private

Definition at line 144 of file cp_forward_obstacle_guard.hpp.

144{false};

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

◆ triggerStreak_

int cl_px4_mr::CpForwardObstacleGuard::triggerStreak_ = 0

private

Definition at line 142 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 139 of file cp_forward_obstacle_guard.hpp.

139{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

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CpForwardObstacleGuard()

◆ ~CpForwardObstacleGuard()

Member Function Documentation

◆ isTooClose()

◆ lastHits()

◆ lastMinRange()

◆ lastYawRate()

◆ levelRotation()

◆ onAttitude()

◆ onCloud()

◆ onInitialize()

◆ onObstacleCleared()

◆ onObstacleTooClose()

◆ params()

◆ reset()

Member Data Documentation

◆ attitudeMutex_

◆ attitudeQ_

◆ attitudeSub_

◆ clearStreak_

◆ cosHalfAngle_

◆ gatedClouds_

◆ haveAttitude_

◆ lastHits_

◆ lastMinRange_

◆ lastYaw_

◆ lastYawStampUs_

◆ onObstacleCleared_

◆ onObstacleTooClose_

◆ params_

◆ subscriber_

◆ tooClose_

◆ triggerStreak_

◆ yawRate_