cl_px4_mr::CpGoalChecker Class Reference

#include <cp_goal_checker.hpp>

Inheritance diagram for cl_px4_mr::CpGoalChecker:

Collaboration diagram for cl_px4_mr::CpGoalChecker:

Public Member Functions
	CpGoalChecker ()
virtual	~CpGoalChecker ()
void	onInitialize () override
void	update () override
void	setGoal (float x, float y, float z, float xy_tolerance=0.5f, float z_tolerance=0.3f)
void	clearGoal ()
bool	isGoalActive () const
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const
Public Member Functions inherited from smacc2::ISmaccUpdatable
	ISmaccUpdatable ()
	ISmaccUpdatable (rclcpp::Duration duration)
void	executeUpdate (rclcpp::Node::SharedPtr node)
void	setUpdatePeriod (rclcpp::Duration duration)

Public Attributes
smacc2::SmaccSignal< void()>	onGoalReached_

Private Attributes
CpVehicleLocalPosition *	localPosition_ = nullptr
float	goalX_ = 0.0f
float	goalY_ = 0.0f
float	goalZ_ = 0.0f
float	xyTolerance_ = 0.5f
float	zTolerance_ = 0.3f
bool	goalActive_ = false

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 Member Functions inherited from smacc2::ISmaccUpdatable
Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 25 of file cp_goal_checker.hpp.

Constructor & Destructor Documentation

CpGoalChecker()

cl_px4_mr::CpGoalChecker::CpGoalChecker

(

)

Definition at line 21 of file cp_goal_checker.cpp.

{}

~CpGoalChecker()

cl_px4_mr::CpGoalChecker::~CpGoalChecker ( )

virtual

Definition at line 23 of file cp_goal_checker.cpp.

{}

Member Function Documentation

clearGoal()

void cl_px4_mr::CpGoalChecker::clearGoal

(

)

Definition at line 63 of file cp_goal_checker.cpp.

{ goalActive_ = false; }

References goalActive_.

Referenced by cl_px4_mr::CbGoToLocation::onExit().

Here is the caller graph for this function:

isGoalActive()

bool cl_px4_mr::CpGoalChecker::isGoalActive

(

)

const

Definition at line 65 of file cp_goal_checker.cpp.

{ return goalActive_; }

References goalActive_.

onInitialize()

void cl_px4_mr::CpGoalChecker::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 25 of file cp_goal_checker.cpp.

{
  this->requiresComponent(localPosition_);
  RCLCPP_INFO(getLogger(), "CpGoalChecker: initialized");
}

References smacc2::ISmaccComponent::getLogger(), localPosition_, and smacc2::ISmaccComponent::requiresComponent().

Here is the call graph for this function:

setGoal()

void cl_px4_mr::CpGoalChecker::setGoal	(	float	x,
		float	y,
		float	z,
		float	xy_tolerance = 0.5f,
		float	z_tolerance = 0.3f )

Definition at line 50 of file cp_goal_checker.cpp.

{
  goalX_ = x;
  goalY_ = y;
  goalZ_ = z;
  xyTolerance_ = xy_tolerance;
  zTolerance_ = z_tolerance;
  goalActive_ = true;
  RCLCPP_INFO(
    getLogger(), "CpGoalChecker: goal set [%.2f, %.2f, %.2f] tol(xy=%.2f z=%.2f)", x, y, z,
    xy_tolerance, z_tolerance);
}

References smacc2::ISmaccComponent::getLogger(), goalActive_, goalX_, goalY_, goalZ_, xyTolerance_, and zTolerance_.

Referenced by cl_px4_mr::CbGoToLocation::onEntry(), and cl_px4_mr::CbTakeOff::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

update()

void cl_px4_mr::CpGoalChecker::update ( )

overridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 31 of file cp_goal_checker.cpp.

{
  if (!goalActive_ || !localPosition_ || !localPosition_->isValid()) return;
 
  float dx = localPosition_->getX() - goalX_;
  float dy = localPosition_->getY() - goalY_;
  float dz = localPosition_->getZ() - goalZ_;
  float xyDist = std::sqrt(dx * dx + dy * dy);
  float zDist = std::abs(dz);
 
  if (xyDist <= xyTolerance_ && zDist <= zTolerance_)
  {
    RCLCPP_INFO(
      getLogger(), "CpGoalChecker: GOAL REACHED (xy_dist=%.2f z_dist=%.2f)", xyDist, zDist);
    goalActive_ = false;
    onGoalReached_();
  }
}

References smacc2::ISmaccComponent::getLogger(), cl_px4_mr::CpVehicleLocalPosition::getX(), cl_px4_mr::CpVehicleLocalPosition::getY(), cl_px4_mr::CpVehicleLocalPosition::getZ(), goalActive_, goalX_, goalY_, goalZ_, cl_px4_mr::CpVehicleLocalPosition::isValid(), localPosition_, onGoalReached_, xyTolerance_, and zTolerance_.

Here is the call graph for this function:

Member Data Documentation

goalActive_

bool cl_px4_mr::CpGoalChecker::goalActive_ = false

private

Definition at line 47 of file cp_goal_checker.hpp.

Referenced by clearGoal(), isGoalActive(), setGoal(), and update().

goalX_

float cl_px4_mr::CpGoalChecker::goalX_ = 0.0f

private

Definition at line 42 of file cp_goal_checker.hpp.

Referenced by setGoal(), and update().

goalY_

float cl_px4_mr::CpGoalChecker::goalY_ = 0.0f

private

Definition at line 43 of file cp_goal_checker.hpp.

Referenced by setGoal(), and update().

goalZ_

float cl_px4_mr::CpGoalChecker::goalZ_ = 0.0f

private

Definition at line 44 of file cp_goal_checker.hpp.

Referenced by setGoal(), and update().

localPosition_

CpVehicleLocalPosition* cl_px4_mr::CpGoalChecker::localPosition_ = nullptr

private

Definition at line 41 of file cp_goal_checker.hpp.

Referenced by onInitialize(), and update().

onGoalReached_

smacc2::SmaccSignal<void()> cl_px4_mr::CpGoalChecker::onGoalReached_

Definition at line 38 of file cp_goal_checker.hpp.

Referenced by cl_px4_mr::CbGoToLocation::onEntry(), cl_px4_mr::CbTakeOff::onEntry(), and update().

xyTolerance_

float cl_px4_mr::CpGoalChecker::xyTolerance_ = 0.5f

private

Definition at line 45 of file cp_goal_checker.hpp.

Referenced by setGoal(), and update().

zTolerance_

float cl_px4_mr::CpGoalChecker::zTolerance_ = 0.3f

private

Definition at line 46 of file cp_goal_checker.hpp.

Referenced by setGoal(), and update().

---

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

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