cl_px4_mr::CpTfBroadcaster Class Reference

#include <cp_tf_broadcaster.hpp>

Inheritance diagram for cl_px4_mr::CpTfBroadcaster:

Collaboration diagram for cl_px4_mr::CpTfBroadcaster:

Public Member Functions
	CpTfBroadcaster (TfBroadcasterParams params={})
virtual	~CpTfBroadcaster ()
void	onInitialize () override
const TfBroadcasterParams &	params () const
Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()
virtual	~ISmaccComponent ()
virtual std::string	getName () const

Private Member Functions
void	onAttitude (const px4_msgs::msg::VehicleAttitude::SharedPtr msg)

Private Attributes
TfBroadcasterParams	params_
CpVehicleLocalPosition *	localPosition_ = nullptr
rclcpp::Subscription< px4_msgs::msg::VehicleAttitude >::SharedPtr	attitudeSub_
std::shared_ptr< tf2_ros::TransformBroadcaster >	broadcaster_
std::shared_ptr< tf2_ros::StaticTransformBroadcaster >	staticBroadcaster_
std::chrono::steady_clock::time_point	lastSent_
bool	firstSent_ = false
rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr	trailPub_
nav_msgs::msg::Path	trail_
std::chrono::steady_clock::time_point	lastTrailSent_

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 72 of file cp_tf_broadcaster.hpp.

Constructor & Destructor Documentation

CpTfBroadcaster()

cl_px4_mr::CpTfBroadcaster::CpTfBroadcaster ( TfBroadcasterParams params = {} )

explicit

Definition at line 44 of file cp_tf_broadcaster.cpp.

: params_(params) {}

~CpTfBroadcaster()

cl_px4_mr::CpTfBroadcaster::~CpTfBroadcaster ( )

virtual

Definition at line 46 of file cp_tf_broadcaster.cpp.

{}

Member Function Documentation

onAttitude()

void cl_px4_mr::CpTfBroadcaster::onAttitude ( const px4_msgs::msg::VehicleAttitude::SharedPtr msg )

private

Definition at line 105 of file cp_tf_broadcaster.cpp.

{
  const auto now = std::chrono::steady_clock::now();
  if (firstSent_ && params_.maxRateHz > 0.0)
  {
    const double elapsed = std::chrono::duration<double>(now - lastSent_).count();
    if (elapsed < 1.0 / params_.maxRateHz)
    {
      return;
    }
  }
  if (!localPosition_->isValid())
  {
    return;
  }
 
  // PX4 quaternion is (w, x, y, z), rotation FRD body -> NED earth
  const tf2::Quaternion qNedFrd(msg->q[1], msg->q[2], msg->q[3], msg->q[0]);
  tf2::Quaternion qEnuFlu = kNedEnuQ * qNedFrd * kFrdFluQ;
  qEnuFlu.normalize();
 
  const auto stamp = this->getNode()->now();
 
  geometry_msgs::msg::TransformStamped base;
  base.header.stamp = stamp;
  base.header.frame_id = params_.mapFrame;
  base.child_frame_id = params_.baseFrame;
  base.transform.translation.x = localPosition_->getY();   // east
  base.transform.translation.y = localPosition_->getX();   // north
  base.transform.translation.z = -localPosition_->getZ();  // up
  base.transform.rotation.x = qEnuFlu.x();
  base.transform.rotation.y = qEnuFlu.y();
  base.transform.rotation.z = qEnuFlu.z();
  base.transform.rotation.w = qEnuFlu.w();
 
  geometry_msgs::msg::TransformStamped follow = base;
  follow.child_frame_id = params_.followFrame;
  follow.transform.rotation.x = 0.0;
  follow.transform.rotation.y = 0.0;
  follow.transform.rotation.z = 0.0;
  follow.transform.rotation.w = 1.0;
 
  broadcaster_->sendTransform(std::vector<geometry_msgs::msg::TransformStamped>{base, follow});
  lastSent_ = now;
 
  if (trailPub_)
  {
    const auto & t = base.transform.translation;
    bool append = trail_.poses.empty();
    if (!append)
    {
      const auto & l = trail_.poses.back().pose.position;
      const double dx = t.x - l.x, dy = t.y - l.y, dz = t.z - l.z;
      append = std::sqrt(dx * dx + dy * dy + dz * dz) >= params_.trailMinStepM;
    }
    if (append)
    {
      geometry_msgs::msg::PoseStamped ps;
      ps.header = base.header;
      ps.pose.position.x = t.x;
      ps.pose.position.y = t.y;
      ps.pose.position.z = t.z;
      ps.pose.orientation = base.transform.rotation;
      trail_.poses.push_back(ps);
      if (trail_.poses.size() > params_.trailMaxPoses)
      {
        trail_.poses.erase(trail_.poses.begin());
      }
      const double sinceTrail = std::chrono::duration<double>(now - lastTrailSent_).count();
      if (params_.trailRateHz <= 0.0 || sinceTrail >= 1.0 / params_.trailRateHz)
      {
        trail_.header.stamp = stamp;
        trailPub_->publish(trail_);
        lastTrailSent_ = now;
      }
    }
  }
  if (!firstSent_)
  {
    firstSent_ = true;
    RCLCPP_INFO(
      getLogger(), "CpTfBroadcaster: first %s -> %s sent (ENU %.2f, %.2f, %.2f)",
      params_.mapFrame.c_str(), params_.baseFrame.c_str(), base.transform.translation.x,
      base.transform.translation.y, base.transform.translation.z);
  }
}

References cl_px4_mr::TfBroadcasterParams::baseFrame, broadcaster_, firstSent_, cl_px4_mr::TfBroadcasterParams::followFrame, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), cl_px4_mr::CpVehicleLocalPosition::getX(), cl_px4_mr::CpVehicleLocalPosition::getY(), cl_px4_mr::CpVehicleLocalPosition::getZ(), cl_px4_mr::CpVehicleLocalPosition::isValid(), lastSent_, lastTrailSent_, localPosition_, cl_px4_mr::TfBroadcasterParams::mapFrame, cl_px4_mr::TfBroadcasterParams::maxRateHz, params_, trail_, cl_px4_mr::TfBroadcasterParams::trailMaxPoses, cl_px4_mr::TfBroadcasterParams::trailMinStepM, trailPub_, and cl_px4_mr::TfBroadcasterParams::trailRateHz.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_px4_mr::CpTfBroadcaster::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 48 of file cp_tf_broadcaster.cpp.

{
  this->requiresComponent(localPosition_);
  if (localPosition_ == nullptr)
  {
    RCLCPP_ERROR(
      getLogger(), "CpTfBroadcaster: CpVehicleLocalPosition not found - no TF will be published");
    return;
  }
 
  auto node = this->getNode();
  broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
  staticBroadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
 
  if (params_.publishSensorFrame)
  {
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = node->now();
    t.header.frame_id = params_.baseFrame;
    t.child_frame_id = params_.sensorFrame;
    t.transform.translation.x = params_.sensorXyz[0];
    t.transform.translation.y = params_.sensorXyz[1];
    t.transform.translation.z = params_.sensorXyz[2];
    tf2::Quaternion q;
    q.setRPY(params_.sensorRpy[0], params_.sensorRpy[1], params_.sensorRpy[2]);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();
    staticBroadcaster_->sendTransform(t);
    RCLCPP_INFO(
      getLogger(), "CpTfBroadcaster: static %s -> %s at FLU (%.3f, %.3f, %.3f)",
      params_.baseFrame.c_str(), params_.sensorFrame.c_str(), params_.sensorXyz[0],
      params_.sensorXyz[1], params_.sensorXyz[2]);
  }
 
  if (params_.publishTrail)
  {
    trailPub_ = node->create_publisher<nav_msgs::msg::Path>(
      params_.trailTopic, rclcpp::QoS(1).transient_local());
    trail_.header.frame_id = params_.mapFrame;
    RCLCPP_INFO(
      getLogger(), "CpTfBroadcaster: publishing the flown trail on %s (every %.2f m, <= %.0f Hz)",
      params_.trailTopic.c_str(), params_.trailMinStepM, params_.trailRateHz);
  }
 
  attitudeSub_ = node->create_subscription<px4_msgs::msg::VehicleAttitude>(
    "/fmu/out/vehicle_attitude", rclcpp::SensorDataQoS(),
    std::bind(&CpTfBroadcaster::onAttitude, this, std::placeholders::_1));
 
  RCLCPP_INFO(
    getLogger(),
    "CpTfBroadcaster: broadcasting %s -> %s (and %s) from /fmu/out/vehicle_attitude at <= %.0f Hz",
    params_.mapFrame.c_str(), params_.baseFrame.c_str(), params_.followFrame.c_str(),
    params_.maxRateHz);
}

References attitudeSub_, cl_px4_mr::TfBroadcasterParams::baseFrame, broadcaster_, cl_px4_mr::TfBroadcasterParams::followFrame, smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), localPosition_, cl_px4_mr::TfBroadcasterParams::mapFrame, cl_px4_mr::TfBroadcasterParams::maxRateHz, onAttitude(), params_, cl_px4_mr::TfBroadcasterParams::publishSensorFrame, cl_px4_mr::TfBroadcasterParams::publishTrail, smacc2::ISmaccComponent::requiresComponent(), cl_px4_mr::TfBroadcasterParams::sensorFrame, cl_px4_mr::TfBroadcasterParams::sensorRpy, cl_px4_mr::TfBroadcasterParams::sensorXyz, staticBroadcaster_, trail_, cl_px4_mr::TfBroadcasterParams::trailMinStepM, trailPub_, cl_px4_mr::TfBroadcasterParams::trailRateHz, and cl_px4_mr::TfBroadcasterParams::trailTopic.

Here is the call graph for this function:

params()

const TfBroadcasterParams & cl_px4_mr::CpTfBroadcaster::params ( ) const

inline

Definition at line 80 of file cp_tf_broadcaster.hpp.

{ return params_; }

References params_.

Member Data Documentation

attitudeSub_

rclcpp::Subscription<px4_msgs::msg::VehicleAttitude>::SharedPtr cl_px4_mr::CpTfBroadcaster::attitudeSub_

private

Definition at line 87 of file cp_tf_broadcaster.hpp.

Referenced by onInitialize().

broadcaster_

std::shared_ptr<tf2_ros::TransformBroadcaster> cl_px4_mr::CpTfBroadcaster::broadcaster_

private

Definition at line 88 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude(), and onInitialize().

firstSent_

bool cl_px4_mr::CpTfBroadcaster::firstSent_ = false

private

Definition at line 91 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude().

lastSent_

std::chrono::steady_clock::time_point cl_px4_mr::CpTfBroadcaster::lastSent_

private

Definition at line 90 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude().

lastTrailSent_

std::chrono::steady_clock::time_point cl_px4_mr::CpTfBroadcaster::lastTrailSent_

private

Definition at line 94 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude().

localPosition_

CpVehicleLocalPosition* cl_px4_mr::CpTfBroadcaster::localPosition_ = nullptr

private

Definition at line 86 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude(), and onInitialize().

params_

TfBroadcasterParams cl_px4_mr::CpTfBroadcaster::params_

private

Definition at line 85 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude(), onInitialize(), and params().

staticBroadcaster_

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> cl_px4_mr::CpTfBroadcaster::staticBroadcaster_

private

Definition at line 89 of file cp_tf_broadcaster.hpp.

Referenced by onInitialize().

trail_

nav_msgs::msg::Path cl_px4_mr::CpTfBroadcaster::trail_

private

Definition at line 93 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude(), and onInitialize().

trailPub_

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr cl_px4_mr::CpTfBroadcaster::trailPub_

private

Definition at line 92 of file cp_tf_broadcaster.hpp.

Referenced by onAttitude(), and onInitialize().

---

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

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