#include <cp_vehicle_local_position.hpp>

Inheritance diagram for cl_px4_mr::CpVehicleLocalPosition:

Collaboration diagram for cl_px4_mr::CpVehicleLocalPosition:

Public Member Functions
	CpVehicleLocalPosition ()

virtual	~CpVehicleLocalPosition ()

void	onInitialize () override

float	getX () const

float	getY () const

float	getZ () const

float	getHeading () const

bool	isValid () const

bool	isHeadingGoodForControl () const

bool	isVelocityValid () const

bool	globalRefValid () const

double	getRefLat () const

double	getRefLon () const

float	getRefAlt () const

uint64_t	getRefTimestamp () const

uint8_t	getXyResetCounter () const

uint8_t	getZResetCounter () const

bool	projectToNed (double lat, double lon, float &x, float &y) const

bool	reprojectFromNed (float x, float y, double &lat, double &lon) const

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

virtual	~ISmaccComponent ()

virtual std::string	getName () const

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

smacc2::SmaccSignal< void()>	onLocalPositionReset_

Private Member Functions
void	onPositionMessage (const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg)

Private Attributes
rclcpp::Subscription< px4_msgs::msg::VehicleLocalPosition >::SharedPtr	subscriber_

float	x_ = 0.0f

float	y_ = 0.0f

float	z_ = 0.0f

float	heading_ = 0.0f

bool	valid_ = false

bool	headingGoodForControl_ = false

bool	velocityValid_ = false

bool	xyGlobal_ = false

bool	zGlobal_ = false

double	refLat_ = 0.0

double	refLon_ = 0.0

float	refAlt_ = 0.0f

uint64_t	refTimestamp_ = 0

uint8_t	xyResetCounter_ = 0

uint8_t	zResetCounter_ = 0

bool	firstMessage_ = true

MapProjection	projection_

std::mutex	mutex_

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 36 of file cp_vehicle_local_position.hpp.

Constructor & Destructor Documentation

◆ CpVehicleLocalPosition()

cl_px4_mr::CpVehicleLocalPosition::CpVehicleLocalPosition ( )

Definition at line 20 of file cp_vehicle_local_position.cpp.

20{}

◆ ~CpVehicleLocalPosition()

cl_px4_mr::CpVehicleLocalPosition::~CpVehicleLocalPosition ( )

virtual

Definition at line 22 of file cp_vehicle_local_position.cpp.

22{}

Member Function Documentation

◆ getHeading()

float cl_px4_mr::CpVehicleLocalPosition::getHeading ( ) const

Definition at line 118 of file cp_vehicle_local_position.cpp.

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

References heading_, and mutex_.

Referenced by cl_px4_mr::CbPx4PathFollowerBase::applyCentering(), cl_px4_mr::CpTrajectorySetpoint::hold(), cl_px4_mr::CbChangeAltitude::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::onEntry(), cl_px4_mr::CbSineAltitudeCruise::onEntry(), cl_px4_mr::CbTakeOff::onEntry(), cl_px4_mr::CbWaitForHeadingStable::onEntry(), cl_px4_mr::CbYawRotate::onEntry(), cl_px4_mr::CbYawScan::onEntry(), and cl_px4_mr::CbYawRotate::update().

Here is the caller graph for this function:

◆ getRefAlt()

float cl_px4_mr::CpVehicleLocalPosition::getRefAlt ( ) const

Definition at line 160 of file cp_vehicle_local_position.cpp.

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

References mutex_, and refAlt_.

◆ getRefLat()

double cl_px4_mr::CpVehicleLocalPosition::getRefLat ( ) const

Definition at line 148 of file cp_vehicle_local_position.cpp.

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

References mutex_, and refLat_.

◆ getRefLon()

double cl_px4_mr::CpVehicleLocalPosition::getRefLon ( ) const

Definition at line 154 of file cp_vehicle_local_position.cpp.

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

References mutex_, and refLon_.

◆ getRefTimestamp()

uint64_t cl_px4_mr::CpVehicleLocalPosition::getRefTimestamp ( ) const

Definition at line 166 of file cp_vehicle_local_position.cpp.

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

References mutex_, and refTimestamp_.

◆ getX()

float cl_px4_mr::CpVehicleLocalPosition::getX ( ) const

Definition at line 100 of file cp_vehicle_local_position.cpp.

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

References mutex_, and x_.

Referenced by cl_px4_mr::CpTrajectorySetpoint::hold(), cl_px4_mr::CpTfBroadcaster::onAttitude(), cl_px4_mr::CbChangeAltitude::onEntry(), cl_px4_mr::CbOrbitLocation::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::onEntry(), cl_px4_mr::CbSineAltitudeCruise::onEntry(), cl_px4_mr::CbSpiralUp::onEntry(), cl_px4_mr::CbTakeOff::onEntry(), cl_px4_mr::CbYawRotate::onEntry(), cl_px4_mr::CbYawScan::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::update(), and cl_px4_mr::CpGoalChecker::update().

Here is the caller graph for this function:

◆ getXyResetCounter()

uint8_t cl_px4_mr::CpVehicleLocalPosition::getXyResetCounter ( ) const

Definition at line 172 of file cp_vehicle_local_position.cpp.

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

References mutex_, and xyResetCounter_.

◆ getY()

float cl_px4_mr::CpVehicleLocalPosition::getY ( ) const

Definition at line 106 of file cp_vehicle_local_position.cpp.

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

References mutex_, and y_.

Referenced by cl_px4_mr::CpTrajectorySetpoint::hold(), cl_px4_mr::CpTfBroadcaster::onAttitude(), cl_px4_mr::CbChangeAltitude::onEntry(), cl_px4_mr::CbOrbitLocation::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::onEntry(), cl_px4_mr::CbSineAltitudeCruise::onEntry(), cl_px4_mr::CbSpiralUp::onEntry(), cl_px4_mr::CbTakeOff::onEntry(), cl_px4_mr::CbYawRotate::onEntry(), cl_px4_mr::CbYawScan::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::update(), and cl_px4_mr::CpGoalChecker::update().

Here is the caller graph for this function:

◆ getZ()

float cl_px4_mr::CpVehicleLocalPosition::getZ ( ) const

Definition at line 112 of file cp_vehicle_local_position.cpp.

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

References mutex_, and z_.

Referenced by cl_px4_mr::CpTrajectorySetpoint::hold(), cl_px4_mr::CpTfBroadcaster::onAttitude(), cl_px4_mr::CbPx4PathFollowerBase::onEntry(), cl_px4_mr::CbSineAltitudeCruise::onEntry(), cl_px4_mr::CbSpiralUp::onEntry(), cl_px4_mr::CbYawRotate::onEntry(), cl_px4_mr::CbYawScan::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::update(), and cl_px4_mr::CpGoalChecker::update().

Here is the caller graph for this function:

◆ getZResetCounter()

uint8_t cl_px4_mr::CpVehicleLocalPosition::getZResetCounter ( ) const

Definition at line 178 of file cp_vehicle_local_position.cpp.

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

References mutex_, and zResetCounter_.

◆ globalRefValid()

bool cl_px4_mr::CpVehicleLocalPosition::globalRefValid ( ) const

Definition at line 142 of file cp_vehicle_local_position.cpp.

{
  std::lock_guard<std::mutex> lock(mutex_);
  return xyGlobal_ && projection_.isInitialized();
}

References cl_px4_mr::MapProjection::isInitialized(), mutex_, projection_, and xyGlobal_.

Here is the call graph for this function:

◆ isHeadingGoodForControl()

bool cl_px4_mr::CpVehicleLocalPosition::isHeadingGoodForControl ( ) const

Definition at line 130 of file cp_vehicle_local_position.cpp.

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

References headingGoodForControl_, and mutex_.

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

Here is the caller graph for this function:

◆ isValid()

bool cl_px4_mr::CpVehicleLocalPosition::isValid ( ) const

Definition at line 124 of file cp_vehicle_local_position.cpp.

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

References mutex_, and valid_.

Referenced by cl_px4_mr::CpTrajectorySetpoint::hold(), cl_px4_mr::CpTfBroadcaster::onAttitude(), cl_px4_mr::CbConnectMicroRosAgent::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::onEntry(), cl_px4_mr::CbSineAltitudeCruise::onEntry(), cl_px4_mr::CbWaitForHeadingStable::onEntry(), cl_px4_mr::CbYawScan::onEntry(), cl_px4_mr::CbPx4PathFollowerBase::update(), and cl_px4_mr::CpGoalChecker::update().

Here is the caller graph for this function:

◆ isVelocityValid()

bool cl_px4_mr::CpVehicleLocalPosition::isVelocityValid ( ) const

Definition at line 136 of file cp_vehicle_local_position.cpp.

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

References mutex_, and velocityValid_.

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

Here is the caller graph for this function:

◆ onInitialize()

void cl_px4_mr::CpVehicleLocalPosition::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 24 of file cp_vehicle_local_position.cpp.

{
  auto node = this->getNode();
  subscriber_ = node->create_subscription<px4_msgs::msg::VehicleLocalPosition>(
    "/fmu/out/vehicle_local_position", rclcpp::SensorDataQoS(),
    std::bind(&CpVehicleLocalPosition::onPositionMessage, this, std::placeholders::_1));
  RCLCPP_INFO(getLogger(), "CpVehicleLocalPosition: subscribed to /fmu/out/vehicle_local_position");
}

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), onPositionMessage(), and subscriber_.

Here is the call graph for this function:

◆ onPositionMessage()

void cl_px4_mr::CpVehicleLocalPosition::onPositionMessage ( const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg )

private

Definition at line 33 of file cp_vehicle_local_position.cpp.

{
  bool resetDetected = false;
 
  {
    std::lock_guard<std::mutex> lock(mutex_);
    x_ = msg->x;
    y_ = msg->y;
    z_ = msg->z;
    heading_ = msg->heading;
    valid_ = msg->xy_valid && msg->z_valid;
    headingGoodForControl_ = msg->heading_good_for_control;
    velocityValid_ = msg->v_xy_valid && msg->v_z_valid;
 
    // global reference of the local frame
    xyGlobal_ = msg->xy_global;
    zGlobal_ = msg->z_global;
    refAlt_ = msg->ref_alt;
    if (msg->xy_global && (!projection_.isInitialized() || msg->ref_timestamp != refTimestamp_))
    {
      refLat_ = msg->ref_lat;
      refLon_ = msg->ref_lon;
      refTimestamp_ = msg->ref_timestamp;
      projection_.initReference(refLat_, refLon_, refTimestamp_);
      RCLCPP_INFO(
        getLogger(),
        "CpVehicleLocalPosition: global reference set - NED origin at lat=%.7f lon=%.7f "
        "alt=%.1f m AMSL",
        refLat_, refLon_, static_cast<double>(refAlt_));
    }
 
    // EKF origin resets
    if (!firstMessage_)
    {
      if (msg->xy_reset_counter != xyResetCounter_)
      {
        RCLCPP_WARN(
          getLogger(),
          "CpVehicleLocalPosition: local XY reset #%u (delta_xy=[%.2f, %.2f]); running paths "
          "are NOT re-projected",
          msg->xy_reset_counter, static_cast<double>(msg->delta_xy[0]),
          static_cast<double>(msg->delta_xy[1]));
        resetDetected = true;
      }
      if (msg->z_reset_counter != zResetCounter_)
      {
        RCLCPP_WARN(
          getLogger(),
          "CpVehicleLocalPosition: local Z reset #%u (delta_z=%.2f); running paths are NOT "
          "re-projected",
          msg->z_reset_counter, static_cast<double>(msg->delta_z));
        resetDetected = true;
      }
    }
    xyResetCounter_ = msg->xy_reset_counter;
    zResetCounter_ = msg->z_reset_counter;
    firstMessage_ = false;
  }
 
  if (resetDetected)
  {
    onLocalPositionReset_();
  }
  onPositionReceived_();
}

References firstMessage_, smacc2::ISmaccComponent::getLogger(), heading_, headingGoodForControl_, cl_px4_mr::MapProjection::initReference(), cl_px4_mr::MapProjection::isInitialized(), mutex_, onLocalPositionReset_, onPositionReceived_, projection_, refAlt_, refLat_, refLon_, refTimestamp_, valid_, velocityValid_, x_, xyGlobal_, xyResetCounter_, y_, z_, zGlobal_, and zResetCounter_.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ projectToNed()

bool cl_px4_mr::CpVehicleLocalPosition::projectToNed	(	double	lat,
		double	lon,
		float &	x,
		float &	y ) const

Definition at line 184 of file cp_vehicle_local_position.cpp.

{
  std::lock_guard<std::mutex> lock(mutex_);
  if (!xyGlobal_ || !projection_.isInitialized())
  {
    return false;
  }
  projection_.project(lat, lon, x, y);
  return true;
}

References cl_px4_mr::MapProjection::isInitialized(), mutex_, cl_px4_mr::MapProjection::project(), projection_, and xyGlobal_.

Here is the call graph for this function:

◆ reprojectFromNed()

bool cl_px4_mr::CpVehicleLocalPosition::reprojectFromNed	(	float	x,
		float	y,
		double &	lat,
		double &	lon ) const

Definition at line 195 of file cp_vehicle_local_position.cpp.

{
  std::lock_guard<std::mutex> lock(mutex_);
  if (!xyGlobal_ || !projection_.isInitialized())
  {
    return false;
  }
  projection_.reproject(x, y, lat, lon);
  return true;
}

References cl_px4_mr::MapProjection::isInitialized(), mutex_, projection_, cl_px4_mr::MapProjection::reproject(), and xyGlobal_.

Here is the call graph for this function:

Member Data Documentation

◆ firstMessage_

bool cl_px4_mr::CpVehicleLocalPosition::firstMessage_ = true

private

Definition at line 92 of file cp_vehicle_local_position.hpp.

Referenced by onPositionMessage().

◆ heading_

float cl_px4_mr::CpVehicleLocalPosition::heading_ = 0.0f

private

Definition at line 79 of file cp_vehicle_local_position.hpp.

Referenced by getHeading(), and onPositionMessage().

◆ headingGoodForControl_

bool cl_px4_mr::CpVehicleLocalPosition::headingGoodForControl_ = false

private

Definition at line 81 of file cp_vehicle_local_position.hpp.

Referenced by isHeadingGoodForControl(), and onPositionMessage().

◆ mutex_

std::mutex cl_px4_mr::CpVehicleLocalPosition::mutex_

mutableprivate

Definition at line 95 of file cp_vehicle_local_position.hpp.

Referenced by getHeading(), getRefAlt(), getRefLat(), getRefLon(), getRefTimestamp(), getX(), getXyResetCounter(), getY(), getZ(), getZResetCounter(), globalRefValid(), isHeadingGoodForControl(), isValid(), isVelocityValid(), onPositionMessage(), projectToNed(), and reprojectFromNed().

◆ onLocalPositionReset_

smacc2::SmaccSignal<void()> cl_px4_mr::CpVehicleLocalPosition::onLocalPositionReset_

Definition at line 70 of file cp_vehicle_local_position.hpp.

Referenced by onPositionMessage().

◆ onPositionReceived_

smacc2::SmaccSignal<void()> cl_px4_mr::CpVehicleLocalPosition::onPositionReceived_

Definition at line 69 of file cp_vehicle_local_position.hpp.

Referenced by onPositionMessage().

◆ projection_

MapProjection cl_px4_mr::CpVehicleLocalPosition::projection_

private

Definition at line 93 of file cp_vehicle_local_position.hpp.

Referenced by globalRefValid(), onPositionMessage(), projectToNed(), and reprojectFromNed().

◆ refAlt_

float cl_px4_mr::CpVehicleLocalPosition::refAlt_ = 0.0f

private

Definition at line 88 of file cp_vehicle_local_position.hpp.

Referenced by getRefAlt(), and onPositionMessage().

◆ refLat_

double cl_px4_mr::CpVehicleLocalPosition::refLat_ = 0.0

private

Definition at line 86 of file cp_vehicle_local_position.hpp.

Referenced by getRefLat(), and onPositionMessage().

◆ refLon_

double cl_px4_mr::CpVehicleLocalPosition::refLon_ = 0.0

private

Definition at line 87 of file cp_vehicle_local_position.hpp.

Referenced by getRefLon(), and onPositionMessage().

◆ refTimestamp_

uint64_t cl_px4_mr::CpVehicleLocalPosition::refTimestamp_ = 0

private

Definition at line 89 of file cp_vehicle_local_position.hpp.

Referenced by getRefTimestamp(), and onPositionMessage().

◆ subscriber_

rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr cl_px4_mr::CpVehicleLocalPosition::subscriber_

private

Definition at line 75 of file cp_vehicle_local_position.hpp.

Referenced by onInitialize().

◆ valid_

bool cl_px4_mr::CpVehicleLocalPosition::valid_ = false

private

Definition at line 80 of file cp_vehicle_local_position.hpp.

Referenced by isValid(), and onPositionMessage().

◆ velocityValid_

bool cl_px4_mr::CpVehicleLocalPosition::velocityValid_ = false

private

Definition at line 82 of file cp_vehicle_local_position.hpp.

Referenced by isVelocityValid(), and onPositionMessage().

◆ x_

float cl_px4_mr::CpVehicleLocalPosition::x_ = 0.0f

private

Definition at line 76 of file cp_vehicle_local_position.hpp.

Referenced by getX(), and onPositionMessage().

◆ xyGlobal_

bool cl_px4_mr::CpVehicleLocalPosition::xyGlobal_ = false

private

Definition at line 84 of file cp_vehicle_local_position.hpp.

Referenced by globalRefValid(), onPositionMessage(), projectToNed(), and reprojectFromNed().

◆ xyResetCounter_

uint8_t cl_px4_mr::CpVehicleLocalPosition::xyResetCounter_ = 0

private

Definition at line 90 of file cp_vehicle_local_position.hpp.

Referenced by getXyResetCounter(), and onPositionMessage().

◆ y_

float cl_px4_mr::CpVehicleLocalPosition::y_ = 0.0f

private

Definition at line 77 of file cp_vehicle_local_position.hpp.

Referenced by getY(), and onPositionMessage().

◆ z_

float cl_px4_mr::CpVehicleLocalPosition::z_ = 0.0f

private

Definition at line 78 of file cp_vehicle_local_position.hpp.

Referenced by getZ(), and onPositionMessage().

◆ zGlobal_

bool cl_px4_mr::CpVehicleLocalPosition::zGlobal_ = false

private

Definition at line 85 of file cp_vehicle_local_position.hpp.

Referenced by onPositionMessage().

◆ zResetCounter_

uint8_t cl_px4_mr::CpVehicleLocalPosition::zResetCounter_ = 0

private

Definition at line 91 of file cp_vehicle_local_position.hpp.

Referenced by getZResetCounter(), and onPositionMessage().

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

smacc2_client_library/cl_px4_mr/include/cl_px4_mr/components/cp_vehicle_local_position.hpp
smacc2_client_library/cl_px4_mr/src/cl_px4_mr/components/cp_vehicle_local_position.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CpVehicleLocalPosition()

◆ ~CpVehicleLocalPosition()

Member Function Documentation

◆ getHeading()

◆ getRefAlt()

◆ getRefLat()

◆ getRefLon()

◆ getRefTimestamp()

◆ getX()

◆ getXyResetCounter()

◆ getY()

◆ getZ()

◆ getZResetCounter()

◆ globalRefValid()

◆ isHeadingGoodForControl()

◆ isValid()

◆ isVelocityValid()

◆ onInitialize()

◆ onPositionMessage()

◆ projectToNed()

◆ reprojectFromNed()

Member Data Documentation

◆ firstMessage_

◆ heading_

◆ headingGoodForControl_

◆ mutex_

◆ onLocalPositionReset_

◆ onPositionReceived_

◆ projection_

◆ refAlt_

◆ refLat_

◆ refLon_

◆ refTimestamp_

◆ subscriber_

◆ valid_

◆ velocityValid_

◆ x_

◆ xyGlobal_

◆ xyResetCounter_

◆ y_

◆ z_

◆ zGlobal_

◆ zResetCounter_