attitude.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <px4_msgs/msg/vehicle_attitude_setpoint.hpp>
#include <px4_ros2/common/setpoint_base.hpp>
 
namespace px4_ros2 {
class AttitudeSetpointType : public SetpointBase {
 public:
  explicit AttitudeSetpointType(Context& context);
 
  ~AttitudeSetpointType() override = default;
 
  SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_ATTITUDE; }
 
  float desiredUpdateRateHz() override { return 100.f; }
 
  void update(const Eigen::Quaternionf& attitude_setpoint,
              const Eigen::Vector3f& thrust_setpoint_frd, float yaw_sp_move_rate_rad_s = 0.f);
 
  void update(float roll, float pitch, float yaw, const Eigen::Vector3f& thrust_setpoint_body,
              float yaw_sp_move_rate_rad_s = 0.f);
 
 private:
  rclcpp::Node& _node;
  rclcpp::Publisher<px4_msgs::msg::VehicleAttitudeSetpoint>::SharedPtr
      _vehicle_attitude_setpoint_pub;
};
 
} /* namespace px4_ros2 */