#include <cb_px4_path_follower_base.hpp>

Inheritance diagram for cl_px4_mr::CbPx4PathFollowerBase:

Collaboration diagram for cl_px4_mr::CbPx4PathFollowerBase:

Public Member Functions
	CbPx4PathFollowerBase (PathFollowerParams params={})

virtual	~CbPx4PathFollowerBase ()

template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

void	setFollowerParams (const PathFollowerParams &params)

const PathFollowerParams &	followerParams () const

void	onEntry () override

void	onExit () override

void	update () override

Public Member Functions inherited from cl_px4_mr::CbPx4ClientBehaviorBase
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

virtual	~CbPx4ClientBehaviorBase ()

void	setTimeout (std::chrono::milliseconds timeout)

bool	hasTimeout () const

Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

virtual	~SmaccAsyncClientBehavior ()

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onSuccess (TCallback callback, T *object)

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onFinished (TCallback callback, T *object)

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onFailure (TCallback callback, T *object)

void	requestForceFinish ()

void	executeOnEntry () override

void	executeOnExit () override

void	waitOnEntryThread (bool requestFinish)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	onSuccess (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	onFinished (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	onFailure (TCallbackMethod callback, T *object)

Public Member Functions inherited from smacc2::ISmaccClientBehavior
	ISmaccClientBehavior ()

virtual	~ISmaccClientBehavior ()

ISmaccStateMachine *	getStateMachine ()

std::string	getName () const

template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)

template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename SmaccComponentType >
void	requiresComponent (std::string name, SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

Public Member Functions inherited from smacc2::ISmaccUpdatable
	ISmaccUpdatable ()

	ISmaccUpdatable (rclcpp::Duration duration)

void	executeUpdate (rclcpp::Node::SharedPtr node)

void	setUpdatePeriod (rclcpp::Duration duration)

Protected Member Functions
virtual std::vector< NedPoint >	buildPath (const NedPoint &current)=0

virtual const char *	behaviorName () const

virtual void	onPathStarted (const std::vector< NedPoint > &)

virtual void	onPathCompleted ()

float	totalLength () const

float	progressFraction () const

size_t	carrotVertexIndex () const

Protected Member Functions inherited from cl_px4_mr::CbPx4ClientBehaviorBase
virtual void	wireCompletionSignals ()

void	postPx4Success ()

void	postPx4Failure ()

Protected Member Functions inherited from smacc2::SmaccAsyncClientBehavior
void	postSuccessEvent ()

void	postFailureEvent ()

virtual void	dispose () override

bool	isShutdownRequested ()
	onEntry is executed in a new thread. However the current state cannot be left until the onEntry thread finishes. This flag can be checked from the onEntry thread to force finishing the thread.

Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

ISmaccState *	getCurrentState ()

virtual rclcpp::Node::SharedPtr	getNode () const

virtual rclcpp::Logger	getLogger () const

Protected Member Functions inherited from smacc2::ISmaccUpdatable

Protected Attributes
PathFollowerParams	followerParams_

CpTunnelCentering *	tunnelCentering_ = nullptr

Protected Attributes inherited from cl_px4_mr::CbPx4ClientBehaviorBase
CpVehicleCommand *	vehicleCommand_ = nullptr

CpVehicleStatus *	vehicleStatus_ = nullptr

CpTrajectorySetpoint *	trajectorySetpoint_ = nullptr

CpGoalChecker *	goalChecker_ = nullptr

CpVehicleLocalPosition *	localPosition_ = nullptr

CpOffboardKeepAlive *	offboardKeepAlive_ = nullptr

Private Member Functions
NedPoint	commandFor (float s)

void	applyCentering (NedPoint &cmd) const

float	tangentYawAt (size_t segmentIndex) const

Private Attributes
std::vector< NedPoint >	path_

std::vector< float >	cumLen_

float	totalLen_ = 0.0f

float	sCarrot_ = 0.0f

std::atomic< size_t >	carrotVertex_ {0}

float	entryHeading_ = 0.0f

NedPoint	lastCmd_

int	lastProgressDecile_ = -1

std::atomic< bool >	active_ {false}

std::chrono::steady_clock::time_point	lastUpdateTime_

Detailed Description

Definition at line 81 of file cb_px4_path_follower_base.hpp.

Constructor & Destructor Documentation

◆ CbPx4PathFollowerBase()

cl_px4_mr::CbPx4PathFollowerBase::CbPx4PathFollowerBase ( PathFollowerParams params = {} )

explicit

Definition at line 33 of file cb_px4_path_follower_base.cpp.

33: followerParams_(params) {}

cl_px4_mr::CbPx4PathFollowerBase::followerParams_

PathFollowerParams followerParams_

Definition cb_px4_path_follower_base.hpp:121

◆ ~CbPx4PathFollowerBase()

virtual cl_px4_mr::CbPx4PathFollowerBase::~CbPx4PathFollowerBase ( )

inlinevirtual

Definition at line 85 of file cb_px4_path_follower_base.hpp.

85{}

Member Function Documentation

◆ applyCentering()

void cl_px4_mr::CbPx4PathFollowerBase::applyCentering ( NedPoint & cmd ) const

private

Definition at line 261 of file cb_px4_path_follower_base.cpp.

{
  if (
    !followerParams_.useTunnelCentering || tunnelCentering_ == nullptr ||
    !tunnelCentering_->valid() || localPosition_ == nullptr)
  {
    return;
  }
  const float up = tunnelCentering_->verticalOffsetM();  // > 0: climb
  cmd.z -= up;
  // the lateral offset is measured in the vehicle's own heading frame (the
  // lidar's); apply it only while that heading agrees with the path tangent -
  // mid-turn "left" can point straight at the wall the path leads away from
  const float heading = localPosition_->getHeading();
  const float misalign = wrapPi(heading - cmd.yaw);
  if (std::fabs(misalign) > 0.7854f)
  {
    return;
  }
  const float lateral = tunnelCentering_->lateralOffsetM();  // > 0: left of the vehicle
  // NED: heading psi from north toward east; left of forward = (sin psi, -cos psi)
  const float s = std::sin(heading);
  const float c = std::cos(heading);
  cmd.x += lateral * s;
  cmd.y -= lateral * c;
}

References followerParams_, cl_px4_mr::CpVehicleLocalPosition::getHeading(), cl_px4_mr::CpTunnelCentering::lateralOffsetM(), cl_px4_mr::CbPx4ClientBehaviorBase::localPosition_, tunnelCentering_, cl_px4_mr::PathFollowerParams::useTunnelCentering, cl_px4_mr::CpTunnelCentering::valid(), cl_px4_mr::CpTunnelCentering::verticalOffsetM(), cl_px4_mr::wrapPi(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Referenced by commandFor().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ behaviorName()

virtual const char * cl_px4_mr::CbPx4PathFollowerBase::behaviorName ( ) const

inlineprotectedvirtual

Reimplemented in cl_px4_mr::CbAscendToAltitude, cl_px4_mr::CbFollowNedPath, cl_px4_mr::CbGridPattern, cl_px4_mr::CbLawnmower, cl_px4_mr::CbLoiter, cl_px4_mr::CbSineWaveHorizontal, cl_px4_mr::CbSineWaveVertical, cl_px4_mr::CbSpiral, cl_px4_mr::CbSquareSpiral, and cl_px4_mr::CbVSSearch.

Definition at line 109 of file cb_px4_path_follower_base.hpp.

109{ return "CbPx4PathFollowerBase"; }

Referenced by onEntry(), onExit(), and update().

Here is the caller graph for this function:

◆ buildPath()

virtual std::vector< NedPoint > cl_px4_mr::CbPx4PathFollowerBase::buildPath ( const NedPoint & current )

protectedpure virtual

Implemented in cl_px4_mr::CbAscendToAltitude, cl_px4_mr::CbFollowNedPath, cl_px4_mr::CbGridPattern, cl_px4_mr::CbLawnmower, cl_px4_mr::CbLoiter, cl_px4_mr::CbSineWaveHorizontal, cl_px4_mr::CbSineWaveVertical, cl_px4_mr::CbSpiral, cl_px4_mr::CbSquareSpiral, and cl_px4_mr::CbVSSearch.

Referenced by onEntry().

Here is the caller graph for this function:

◆ carrotVertexIndex()

size_t cl_px4_mr::CbPx4PathFollowerBase::carrotVertexIndex ( ) const

inlineprotected

Definition at line 119 of file cb_px4_path_follower_base.hpp.

119{ return carrotVertex_.load(); }

cl_px4_mr::CbPx4PathFollowerBase::carrotVertex_

std::atomic< size_t > carrotVertex_

Definition cb_px4_path_follower_base.hpp:133

References carrotVertex_.

Referenced by cl_px4_mr::CbFollowNedPath::reachedCount().

Here is the caller graph for this function:

◆ commandFor()

NedPoint cl_px4_mr::CbPx4PathFollowerBase::commandFor ( float s )

private

Definition at line 232 of file cb_px4_path_follower_base.cpp.

{
  size_t segment = 0;
  NedPoint cmd = sampleAtArcLength(path_, cumLen_, s, &segment);
 
  switch (followerParams_.yawMode)
  {
    case YawMode::FIXED:
      cmd.yaw = std::isnan(followerParams_.fixedYaw) ? entryHeading_ : followerParams_.fixedYaw;
      break;
    case YawMode::HOLD_ENTRY:
      cmd.yaw = entryHeading_;
      break;
    case YawMode::PER_VERTEX:
      if (std::isnan(cmd.yaw))
      {
        cmd.yaw = tangentYawAt(segment);
      }
      break;
    case YawMode::TANGENT:
    default:
      cmd.yaw = tangentYawAt(segment);
      break;
  }
  cmd.yaw = wrapPi(cmd.yaw);
  applyCentering(cmd);
  return cmd;
}

References applyCentering(), cumLen_, entryHeading_, cl_px4_mr::FIXED, cl_px4_mr::PathFollowerParams::fixedYaw, followerParams_, cl_px4_mr::HOLD_ENTRY, path_, cl_px4_mr::PER_VERTEX, cl_px4_mr::sampleAtArcLength(), cl_px4_mr::TANGENT, tangentYawAt(), cl_px4_mr::wrapPi(), cl_px4_mr::NedPoint::yaw, and cl_px4_mr::PathFollowerParams::yawMode.

Referenced by onEntry(), and update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ followerParams()

const PathFollowerParams & cl_px4_mr::CbPx4PathFollowerBase::followerParams ( ) const

inline

Definition at line 96 of file cb_px4_path_follower_base.hpp.

96{ return followerParams_; }

References followerParams_.

◆ onEntry()

void cl_px4_mr::CbPx4PathFollowerBase::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 35 of file cb_px4_path_follower_base.cpp.

{
  if (localPosition_ == nullptr || trajectorySetpoint_ == nullptr)
  {
    RCLCPP_ERROR(
      getLogger(), "%s: local position / trajectory setpoint component missing - posting failure",
      behaviorName());
    this->postPx4Failure();
    return;
  }
  if (!localPosition_->isValid())
  {
    RCLCPP_ERROR(
      getLogger(), "%s: no valid local position at entry - posting failure", behaviorName());
    this->postPx4Failure();
    return;
  }
 
  NedPoint current;
  current.x = localPosition_->getX();
  current.y = localPosition_->getY();
  current.z = localPosition_->getZ();
  current.yaw = localPosition_->getHeading();
  entryHeading_ = current.yaw;
 
  std::vector<NedPoint> raw = buildPath(current);
  if (raw.empty())
  {
    RCLCPP_ERROR(
      getLogger(), "%s: buildPath() returned no vertices - posting failure", behaviorName());
    this->postPx4Failure();
    return;
  }
 
  path_.clear();
  if (followerParams_.prependCurrentPosition)
  {
    NedPoint start = current;
    start.yaw = std::numeric_limits<float>::quiet_NaN();
    path_.push_back(start);
  }
  for (const NedPoint & v : raw)
  {
    if (!path_.empty() && nedDistance(path_.back(), v) < followerParams_.minSegmentLength)
    {
      // keep the later vertex's yaw, drop the degenerate segment
      path_.back().yaw = std::isnan(v.yaw) ? path_.back().yaw : v.yaw;
      continue;
    }
    path_.push_back(v);
  }
 
  cumLen_ = cumulativeLengths(path_);
  totalLen_ = cumLen_.empty() ? 0.0f : cumLen_.back();
  sCarrot_ = 0.0f;
  carrotVertex_ = 0;
  lastProgressDecile_ = -1;
 
  const float speed = std::max(followerParams_.groundSpeed, 0.05f);
  const float lag = speed / 0.95f;
  if (followerParams_.leash < 1.2f * lag)
  {
    RCLCPP_WARN(
      getLogger(),
      "%s: leash %.1f m is below 1.2x the expected tracking lag (%.1f m at %.1f m/s) - effective "
      "speed will be throttled to ~%.1f m/s",
      behaviorName(), followerParams_.leash, lag, speed, 0.95f * followerParams_.leash);
  }
 
  if (!this->hasTimeout() && followerParams_.autoTimeoutFactor > 0.0f)
  {
    const float seconds = std::max(30.0f, followerParams_.autoTimeoutFactor * totalLen_ / speed);
    this->setTimeout(std::chrono::milliseconds(static_cast<int64_t>(seconds * 1000.0f)));
    RCLCPP_INFO(getLogger(), "%s: auto-timeout armed at %.0f s", behaviorName(), seconds);
  }
 
  RCLCPP_INFO(
    getLogger(),
    "%s: following %zu vertices, %.1f m at %.1f m/s (leash %.1f m) from NED (%.1f, %.1f, %.1f) "
    "to (%.1f, %.1f, %.1f)%s",
    behaviorName(), path_.size(), totalLen_, speed, followerParams_.leash, path_.front().x,
    path_.front().y, path_.front().z, path_.back().x, path_.back().y, path_.back().z,
    followerParams_.useTunnelCentering
      ? (tunnelCentering_ ? " + tunnel centering"
                          : " (tunnel centering requested but no component)")
      : "");
  onPathStarted(path_);
 
  lastCmd_ = commandFor(0.0f);
  trajectorySetpoint_->setPositionNED(lastCmd_.x, lastCmd_.y, lastCmd_.z, lastCmd_.yaw);
  lastUpdateTime_ = std::chrono::steady_clock::now();
  active_ = true;
}

References active_, cl_px4_mr::PathFollowerParams::autoTimeoutFactor, behaviorName(), buildPath(), carrotVertex_, commandFor(), cumLen_, cl_px4_mr::cumulativeLengths(), entryHeading_, followerParams_, cl_px4_mr::CpVehicleLocalPosition::getHeading(), smacc2::ISmaccClientBehavior::getLogger(), cl_px4_mr::CpVehicleLocalPosition::getX(), cl_px4_mr::CpVehicleLocalPosition::getY(), cl_px4_mr::CpVehicleLocalPosition::getZ(), cl_px4_mr::PathFollowerParams::groundSpeed, cl_px4_mr::CbPx4ClientBehaviorBase::hasTimeout(), cl_px4_mr::CpVehicleLocalPosition::isValid(), lastCmd_, lastProgressDecile_, lastUpdateTime_, cl_px4_mr::PathFollowerParams::leash, cl_px4_mr::CbPx4ClientBehaviorBase::localPosition_, cl_px4_mr::PathFollowerParams::minSegmentLength, cl_px4_mr::nedDistance(), onPathStarted(), path_, cl_px4_mr::CbPx4ClientBehaviorBase::postPx4Failure(), cl_px4_mr::PathFollowerParams::prependCurrentPosition, sCarrot_, cl_px4_mr::CpTrajectorySetpoint::setPositionNED(), cl_px4_mr::CbPx4ClientBehaviorBase::setTimeout(), totalLen_, cl_px4_mr::CbPx4ClientBehaviorBase::trajectorySetpoint_, tunnelCentering_, cl_px4_mr::PathFollowerParams::useTunnelCentering, cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Here is the call graph for this function:

◆ onExit()

void cl_px4_mr::CbPx4PathFollowerBase::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 129 of file cb_px4_path_follower_base.cpp.

{
  if (active_.exchange(false))
  {
    // hold at the last carrot (not hold(): that would step the setpoint back
    // to the lagging vehicle position)
    trajectorySetpoint_->setPositionNED(lastCmd_.x, lastCmd_.y, lastCmd_.z, lastCmd_.yaw);
    RCLCPP_INFO(
      getLogger(), "%s: exiting at %.0f%% - holding at last setpoint (%.1f, %.1f, %.1f)",
      behaviorName(), 100.0f * progressFraction(), lastCmd_.x, lastCmd_.y, lastCmd_.z);
  }
}

References active_, behaviorName(), smacc2::ISmaccClientBehavior::getLogger(), lastCmd_, progressFraction(), cl_px4_mr::CpTrajectorySetpoint::setPositionNED(), cl_px4_mr::CbPx4ClientBehaviorBase::trajectorySetpoint_, cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Here is the call graph for this function:

◆ onPathCompleted()

virtual void cl_px4_mr::CbPx4PathFollowerBase::onPathCompleted ( )

inlineprotectedvirtual

Definition at line 111 of file cb_px4_path_follower_base.hpp.

111{}

Referenced by update().

Here is the caller graph for this function:

◆ onPathStarted()

virtual void cl_px4_mr::CbPx4PathFollowerBase::onPathStarted ( const std::vector< NedPoint > & )

inlineprotectedvirtual

Definition at line 110 of file cb_px4_path_follower_base.hpp.

110{}

Referenced by onEntry().

Here is the caller graph for this function:

◆ onStateOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void cl_px4_mr::CbPx4PathFollowerBase::onStateOrthogonalAllocation ( )

inline

Definition at line 88 of file cb_px4_path_follower_base.hpp.

  {
    // optional, may live in another orthogonal (state machine thread, see the locking rule)
    this->requiresComponent(tunnelCentering_, smacc2::ComponentRequirement::SOFT);
    CbPx4ClientBehaviorBase::onStateOrthogonalAllocation<TOrthogonal, TSourceObject>();
  }

References cl_px4_mr::CbPx4ClientBehaviorBase::onStateOrthogonalAllocation(), smacc2::ISmaccClientBehavior::requiresComponent(), smacc2::SOFT, and tunnelCentering_.

Here is the call graph for this function:

◆ progressFraction()

float cl_px4_mr::CbPx4PathFollowerBase::progressFraction ( ) const

inlineprotected

Definition at line 114 of file cb_px4_path_follower_base.hpp.

114{ return totalLen_ > 0.0f ? sCarrot_ / totalLen_ : 1.0f; }

References sCarrot_, and totalLen_.

Referenced by onExit(), and update().

Here is the caller graph for this function:

◆ setFollowerParams()

void cl_px4_mr::CbPx4PathFollowerBase::setFollowerParams ( const PathFollowerParams & params )

inline

Definition at line 95 of file cb_px4_path_follower_base.hpp.

95{ followerParams_ = params; }

References followerParams_.

◆ tangentYawAt()

float cl_px4_mr::CbPx4PathFollowerBase::tangentYawAt ( size_t segmentIndex ) const

private

Definition at line 214 of file cb_px4_path_follower_base.cpp.

{
  if (path_.size() < 2)
  {
    return entryHeading_;
  }
  const size_t i0 = std::min(segmentIndex, path_.size() - 2);
  const NedPoint & a = path_[i0];
  const NedPoint & b = path_[i0 + 1];
  const float dx = b.x - a.x;
  const float dy = b.y - a.y;
  if (std::hypot(dx, dy) < 1e-3f)
  {
    return entryHeading_;  // vertical segment: keep heading
  }
  return std::atan2(dy, dx);
}

References entryHeading_, path_, cl_px4_mr::NedPoint::x, and cl_px4_mr::NedPoint::y.

Referenced by commandFor().

Here is the caller graph for this function:

◆ totalLength()

float cl_px4_mr::CbPx4PathFollowerBase::totalLength ( ) const

inlineprotected

Definition at line 113 of file cb_px4_path_follower_base.hpp.

113{ return totalLen_; }

References totalLen_.

◆ update()

void cl_px4_mr::CbPx4PathFollowerBase::update ( )

overridevirtual

Reimplemented from cl_px4_mr::CbPx4ClientBehaviorBase.

Definition at line 142 of file cb_px4_path_follower_base.cpp.

{
  CbPx4ClientBehaviorBase::update();
 
  if (!active_)
  {
    return;
  }
 
  const auto now = std::chrono::steady_clock::now();
  float dt = std::chrono::duration<float>(now - lastUpdateTime_).count();
  lastUpdateTime_ = now;
  dt = std::clamp(dt, 0.0f, 0.5f);
 
  if (!localPosition_->isValid())
  {
    // freeze the carrot; the watchdog covers a lost position
    return;
  }
 
  NedPoint vehicle;
  vehicle.x = localPosition_->getX();
  vehicle.y = localPosition_->getY();
  vehicle.z = localPosition_->getZ();
 
  // advance the carrot, throttled by the leash (measured to the carrot as it
  // will actually be commanded, centering offsets included)
  const float sNext = std::min(sCarrot_ + followerParams_.groundSpeed * dt, totalLen_);
  const NedPoint candidate = commandFor(sNext);
  if (nedDistance(candidate, vehicle) <= followerParams_.leash)
  {
    sCarrot_ = sNext;
  }
 
  lastCmd_ = commandFor(sCarrot_);
  trajectorySetpoint_->setPositionNED(lastCmd_.x, lastCmd_.y, lastCmd_.z, lastCmd_.yaw);
 
  // vertex most recently passed by the carrot (cumLen_[0] == 0, so the
  // upper_bound is never begin())
  {
    const size_t ub = std::upper_bound(cumLen_.begin(), cumLen_.end(), sCarrot_) - cumLen_.begin();
    const size_t idx = ub == 0 ? 0 : ub - 1;
    carrotVertex_ = path_.empty() ? 0 : std::min(idx, path_.size() - 1);
  }
 
  const int decile = static_cast<int>(progressFraction() * 10.0f);
  if (decile != lastProgressDecile_ && decile > 0 && decile < 10)
  {
    lastProgressDecile_ = decile;
    RCLCPP_INFO(
      getLogger(), "%s: %d%% (%.0f / %.0f m)", behaviorName(), decile * 10, sCarrot_, totalLen_);
  }
 
  // completion: carrot at the end and vehicle within tolerance of the final
  // commanded carrot (== the last vertex unless centering offsets it)
  if (sCarrot_ >= totalLen_)
  {
    const NedPoint & end = lastCmd_;
    const float dxy = nedDistanceXY(vehicle, end);
    const float dz = std::fabs(vehicle.z - end.z);
    if (dxy <= followerParams_.arrivalXyTol && dz <= followerParams_.arrivalZTol)
    {
      active_ = false;
      RCLCPP_INFO(
        getLogger(), "%s: path complete (xy err %.2f m, z err %.2f m) - posting success",
        behaviorName(), dxy, dz);
      onPathCompleted();
      this->postPx4Success();
    }
  }
}

References active_, cl_px4_mr::PathFollowerParams::arrivalXyTol, cl_px4_mr::PathFollowerParams::arrivalZTol, behaviorName(), carrotVertex_, commandFor(), cumLen_, followerParams_, smacc2::ISmaccClientBehavior::getLogger(), cl_px4_mr::CpVehicleLocalPosition::getX(), cl_px4_mr::CpVehicleLocalPosition::getY(), cl_px4_mr::CpVehicleLocalPosition::getZ(), cl_px4_mr::PathFollowerParams::groundSpeed, cl_px4_mr::CpVehicleLocalPosition::isValid(), lastCmd_, lastProgressDecile_, lastUpdateTime_, cl_px4_mr::PathFollowerParams::leash, cl_px4_mr::CbPx4ClientBehaviorBase::localPosition_, cl_px4_mr::nedDistance(), cl_px4_mr::nedDistanceXY(), onPathCompleted(), path_, cl_px4_mr::CbPx4ClientBehaviorBase::postPx4Success(), progressFraction(), sCarrot_, cl_px4_mr::CpTrajectorySetpoint::setPositionNED(), totalLen_, cl_px4_mr::CbPx4ClientBehaviorBase::trajectorySetpoint_, cl_px4_mr::CbPx4ClientBehaviorBase::update(), cl_px4_mr::NedPoint::x, cl_px4_mr::NedPoint::y, cl_px4_mr::NedPoint::yaw, and cl_px4_mr::NedPoint::z.

Here is the call graph for this function:

Member Data Documentation

◆ active_

std::atomic<bool> cl_px4_mr::CbPx4PathFollowerBase::active_ {false}

private

Definition at line 137 of file cb_px4_path_follower_base.hpp.

137{false};

Referenced by onEntry(), onExit(), and update().

◆ carrotVertex_

std::atomic<size_t> cl_px4_mr::CbPx4PathFollowerBase::carrotVertex_ {0}

private

Definition at line 133 of file cb_px4_path_follower_base.hpp.

133{0};

Referenced by carrotVertexIndex(), onEntry(), and update().

◆ cumLen_

std::vector<float> cl_px4_mr::CbPx4PathFollowerBase::cumLen_

private

Definition at line 130 of file cb_px4_path_follower_base.hpp.

Referenced by commandFor(), onEntry(), and update().

◆ entryHeading_

float cl_px4_mr::CbPx4PathFollowerBase::entryHeading_ = 0.0f

private

Definition at line 134 of file cb_px4_path_follower_base.hpp.

Referenced by commandFor(), onEntry(), and tangentYawAt().

◆ followerParams_

PathFollowerParams cl_px4_mr::CbPx4PathFollowerBase::followerParams_

protected

Definition at line 121 of file cb_px4_path_follower_base.hpp.

◆ lastCmd_

NedPoint cl_px4_mr::CbPx4PathFollowerBase::lastCmd_

private

Definition at line 135 of file cb_px4_path_follower_base.hpp.

Referenced by onEntry(), onExit(), and update().

◆ lastProgressDecile_

int cl_px4_mr::CbPx4PathFollowerBase::lastProgressDecile_ = -1

private

Definition at line 136 of file cb_px4_path_follower_base.hpp.

Referenced by onEntry(), and update().

◆ lastUpdateTime_

std::chrono::steady_clock::time_point cl_px4_mr::CbPx4PathFollowerBase::lastUpdateTime_

private

Definition at line 138 of file cb_px4_path_follower_base.hpp.

Referenced by onEntry(), and update().

◆ path_

std::vector<NedPoint> cl_px4_mr::CbPx4PathFollowerBase::path_

private

Definition at line 129 of file cb_px4_path_follower_base.hpp.

Referenced by commandFor(), onEntry(), tangentYawAt(), and update().

◆ sCarrot_

float cl_px4_mr::CbPx4PathFollowerBase::sCarrot_ = 0.0f

private

Definition at line 132 of file cb_px4_path_follower_base.hpp.

Referenced by onEntry(), progressFraction(), and update().

◆ totalLen_

float cl_px4_mr::CbPx4PathFollowerBase::totalLen_ = 0.0f

private

Definition at line 131 of file cb_px4_path_follower_base.hpp.

Referenced by onEntry(), progressFraction(), totalLength(), and update().

◆ tunnelCentering_

CpTunnelCentering* cl_px4_mr::CbPx4PathFollowerBase::tunnelCentering_ = nullptr

protected

Definition at line 122 of file cb_px4_path_follower_base.hpp.

Referenced by applyCentering(), onEntry(), and onStateOrthogonalAllocation().

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

smacc2_client_library/cl_px4_mr/include/cl_px4_mr/client_behaviors/cb_px4_path_follower_base.hpp
smacc2_client_library/cl_px4_mr/src/cl_px4_mr/client_behaviors/cb_px4_path_follower_base.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CbPx4PathFollowerBase()

◆ ~CbPx4PathFollowerBase()

Member Function Documentation

◆ applyCentering()

◆ behaviorName()

◆ buildPath()

◆ carrotVertexIndex()

◆ commandFor()

◆ followerParams()

◆ onEntry()

◆ onExit()

◆ onPathCompleted()

◆ onPathStarted()

◆ onStateOrthogonalAllocation()

◆ progressFraction()

◆ setFollowerParams()

◆ tangentYawAt()

◆ totalLength()

◆ update()

Member Data Documentation

◆ active_

◆ carrotVertex_

◆ cumLen_

◆ entryHeading_

◆ followerParams_

◆ lastCmd_

◆ lastProgressDecile_

◆ lastUpdateTime_

◆ path_

◆ sCarrot_

◆ totalLen_

◆ tunnelCentering_