direct_actuators.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Core>
#include <px4_msgs/msg/actuator_motors.hpp>
#include <px4_msgs/msg/actuator_servos.hpp>
#include <px4_ros2/common/setpoint_base.hpp>
 
namespace px4_ros2 {
class DirectActuatorsSetpointType : public SetpointBase {
 public:
  explicit DirectActuatorsSetpointType(Context& context);
 
  static constexpr int kMaxNumMotors = px4_msgs::msg::ActuatorMotors::NUM_CONTROLS;
  static constexpr int kMaxNumServos = px4_msgs::msg::ActuatorServos::NUM_CONTROLS;
 
  ~DirectActuatorsSetpointType() override = default;
 
  SetpointType getSetpointType() override
  {
    return px4_msgs::msg::SetpointConfig::TYPE_DIRECT_ACTUATORS;
  }
 
  float desiredUpdateRateHz() override { return 200.f; }
 
  void updateServos(const Eigen::Matrix<float, kMaxNumServos, 1>& servo_commands);
 
  void updateMotors(const Eigen::Matrix<float, kMaxNumMotors, 1>& motor_commands);
 
 private:
  rclcpp::Node& _node;
  rclcpp::Publisher<px4_msgs::msg::ActuatorMotors>::SharedPtr _actuator_motors_pub;
  rclcpp::Publisher<px4_msgs::msg::ActuatorServos>::SharedPtr _actuator_servos_pub;
};
 
} /* namespace px4_ros2 */