cl_px4_mr::CpTrajectorySetpoint Class Reference

#include <cp_trajectory_setpoint.hpp>

Inheritance diagram for cl_px4_mr::CpTrajectorySetpoint:

Collaboration diagram for cl_px4_mr::CpTrajectorySetpoint:

Public Member Functions
	CpTrajectorySetpoint ()
virtual	~CpTrajectorySetpoint ()
void	onInitialize () override
void	setPositionNED (float x, float y, float z, float yaw=std::numeric_limits< float >::quiet_NaN())
void	hold ()
void	republishLast ()
px4_msgs::msg::TrajectorySetpoint	getLastSetpoint () const
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Private Attributes
rclcpp::Publisher< px4_msgs::msg::TrajectorySetpoint >::SharedPtr	publisher_
CpVehicleLocalPosition *	localPosition_ = nullptr
px4_msgs::msg::TrajectorySetpoint	lastSetpoint_
std::mutex	mutex_
bool	hasPublished_ = 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 Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 28 of file cp_trajectory_setpoint.hpp.

Constructor & Destructor Documentation

CpTrajectorySetpoint()

cl_px4_mr::CpTrajectorySetpoint::CpTrajectorySetpoint

(

)

Definition at line 21 of file cp_trajectory_setpoint.cpp.

{}

~CpTrajectorySetpoint()

cl_px4_mr::CpTrajectorySetpoint::~CpTrajectorySetpoint ( )

virtual

Definition at line 23 of file cp_trajectory_setpoint.cpp.

{}

Member Function Documentation

getLastSetpoint()

px4_msgs::msg::TrajectorySetpoint cl_px4_mr::CpTrajectorySetpoint::getLastSetpoint

(

)

const

Definition at line 95 of file cp_trajectory_setpoint.cpp.

{
  std::lock_guard<std::mutex> lock(mutex_);
  return lastSetpoint_;
}

References lastSetpoint_, and mutex_.

hold()

void cl_px4_mr::CpTrajectorySetpoint::hold

(

)

Definition at line 72 of file cp_trajectory_setpoint.cpp.

{
  if (!localPosition_ || !localPosition_->isValid())
  {
    RCLCPP_WARN(getLogger(), "CpTrajectorySetpoint::hold() - position not yet valid, skipping");
    return;
  }
 
  setPositionNED(
    localPosition_->getX(), localPosition_->getY(), localPosition_->getZ(),
    localPosition_->getHeading());
}

References cl_px4_mr::CpVehicleLocalPosition::getHeading(), smacc2::ISmaccComponent::getLogger(), cl_px4_mr::CpVehicleLocalPosition::getX(), cl_px4_mr::CpVehicleLocalPosition::getY(), cl_px4_mr::CpVehicleLocalPosition::getZ(), cl_px4_mr::CpVehicleLocalPosition::isValid(), localPosition_, and setPositionNED().

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

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_px4_mr::CpTrajectorySetpoint::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 25 of file cp_trajectory_setpoint.cpp.

{
  auto node = this->getNode();
  publisher_ = node->create_publisher<px4_msgs::msg::TrajectorySetpoint>(
    "/fmu/in/trajectory_setpoint", rclcpp::QoS(1).best_effort());
 
  this->requiresComponent(localPosition_);
 
  // Initialize lastSetpoint_ with NaNs
  auto nan = std::numeric_limits<float>::quiet_NaN();
  lastSetpoint_.position = {nan, nan, nan};
  lastSetpoint_.velocity = {nan, nan, nan};
  lastSetpoint_.acceleration = {nan, nan, nan};
  lastSetpoint_.jerk = {nan, nan, nan};
  lastSetpoint_.yaw = nan;
  lastSetpoint_.yawspeed = nan;
 
  RCLCPP_INFO(
    getLogger(), "CpTrajectorySetpoint: publisher created on /fmu/in/trajectory_setpoint");
}

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

Here is the call graph for this function:

republishLast()

void cl_px4_mr::CpTrajectorySetpoint::republishLast

(

)

Definition at line 85 of file cp_trajectory_setpoint.cpp.

{
  std::lock_guard<std::mutex> lock(mutex_);
  if (!hasPublished_) return;
 
  auto node = this->getNode();
  lastSetpoint_.timestamp = node->get_clock()->now().nanoseconds() / 1000;
  publisher_->publish(lastSetpoint_);
}

References smacc2::ISmaccComponent::getNode(), hasPublished_, lastSetpoint_, mutex_, and publisher_.

Referenced by cl_px4_mr::CpOffboardKeepAlive::update().

Here is the call graph for this function:

Here is the caller graph for this function:

setPositionNED()

void cl_px4_mr::CpTrajectorySetpoint::setPositionNED	(	float	x,
		float	y,
		float	z,
		float	yaw = std::numeric_limits<float>::quiet_NaN() )

Definition at line 46 of file cp_trajectory_setpoint.cpp.

{
  auto node = this->getNode();
  auto nan = std::numeric_limits<float>::quiet_NaN();
 
  px4_msgs::msg::TrajectorySetpoint msg;
  msg.timestamp = node->get_clock()->now().nanoseconds() / 1000;
  msg.position = {x, y, z};
  msg.velocity = {nan, nan, nan};
  msg.acceleration = {nan, nan, nan};
  msg.jerk = {nan, nan, nan};
  msg.yaw = yaw;
  msg.yawspeed = nan;
 
  publisher_->publish(msg);
 
  {
    std::lock_guard<std::mutex> lock(mutex_);
    lastSetpoint_ = msg;
    hasPublished_ = true;
  }
 
  RCLCPP_INFO(
    getLogger(), "CpTrajectorySetpoint: position NED [%.2f, %.2f, %.2f] yaw=%.2f", x, y, z, yaw);
}

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), hasPublished_, lastSetpoint_, mutex_, and publisher_.

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

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

hasPublished_

bool cl_px4_mr::CpTrajectorySetpoint::hasPublished_ = false

private

Definition at line 48 of file cp_trajectory_setpoint.hpp.

Referenced by republishLast(), and setPositionNED().

lastSetpoint_

px4_msgs::msg::TrajectorySetpoint cl_px4_mr::CpTrajectorySetpoint::lastSetpoint_

private

Definition at line 46 of file cp_trajectory_setpoint.hpp.

Referenced by getLastSetpoint(), onInitialize(), republishLast(), and setPositionNED().

localPosition_

CpVehicleLocalPosition* cl_px4_mr::CpTrajectorySetpoint::localPosition_ = nullptr

private

Definition at line 45 of file cp_trajectory_setpoint.hpp.

Referenced by hold(), and onInitialize().

mutex_

std::mutex cl_px4_mr::CpTrajectorySetpoint::mutex_

mutableprivate

Definition at line 47 of file cp_trajectory_setpoint.hpp.

Referenced by getLastSetpoint(), republishLast(), and setPositionNED().

publisher_

rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr cl_px4_mr::CpTrajectorySetpoint::publisher_

private

Definition at line 44 of file cp_trajectory_setpoint.hpp.

Referenced by onInitialize(), republishLast(), and setPositionNED().

---

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

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