trajectory.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <px4_ros2/common/setpoint_base.hpp>
 
namespace px4_ros2 {
struct TrajectorySetpoint;
 
class TrajectorySetpointType : public SetpointBase {
 public:
  explicit TrajectorySetpointType(Context& context, bool local_position_is_optional = false);
 
  ~TrajectorySetpointType() override = default;
 
  SetpointType getSetpointType() override { return px4_msgs::msg::SetpointConfig::TYPE_TRAJECTORY; }
 
  void clearOptionalRequirements(
      px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) override;
 
  void update(const Eigen::Vector3f& velocity_ned_m_s,
              const std::optional<Eigen::Vector3f>& acceleration_ned_m_s2 = {},
              std::optional<float> yaw_ned_rad = {}, std::optional<float> yaw_rate_ned_rad_s = {});
 
  void update(const TrajectorySetpoint& setpoint);
 
  void updatePosition(const Eigen::Vector3f& position_ned_m);
 
 private:
  rclcpp::Node& _node;
  rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr _trajectory_setpoint_pub;
 
  const bool _local_position_is_optional;
};
 
struct TrajectorySetpoint {
  std::optional<float> position_ned_m_x;
  std::optional<float> position_ned_m_y;
  std::optional<float> position_ned_m_z;
 
  std::optional<float> velocity_ned_m_s_x;
  std::optional<float> velocity_ned_m_s_y;
  std::optional<float> velocity_ned_m_s_z;
 
  std::optional<float> acceleration_ned_m_s2_x;
  std::optional<float> acceleration_ned_m_s2_y;
  std::optional<float> acceleration_ned_m_s2_z;
 
  std::optional<float> yaw_ned_rad;
  std::optional<float> yaw_rate_ned_rad_s;
 
  TrajectorySetpoint& withPositionX(float x_ned_m)
  {
    position_ned_m_x = x_ned_m;
    return *this;
  }
 
  TrajectorySetpoint& withPositionY(float y_ned_m)
  {
    position_ned_m_y = y_ned_m;
    return *this;
  }
 
  TrajectorySetpoint& withPositionZ(float z_ned_m)
  {
    position_ned_m_z = z_ned_m;
    return *this;
  }
 
  TrajectorySetpoint& withVelocityX(float x_ned_m_s)
  {
    velocity_ned_m_s_x = x_ned_m_s;
    return *this;
  }
 
  TrajectorySetpoint& withVelocityY(float y_ned_m_s)
  {
    velocity_ned_m_s_y = y_ned_m_s;
    return *this;
  }
 
  TrajectorySetpoint& withVelocityZ(float z_ned_m_s)
  {
    velocity_ned_m_s_z = z_ned_m_s;
    return *this;
  }
 
  TrajectorySetpoint& withAccelerationX(float x_ned_m_s2)
  {
    acceleration_ned_m_s2_x = x_ned_m_s2;
    return *this;
  }
 
  TrajectorySetpoint& withAccelerationY(float y_ned_m_s2)
  {
    acceleration_ned_m_s2_y = y_ned_m_s2;
    return *this;
  }
 
  TrajectorySetpoint& withAccelerationZ(float z_ned_m_s2)
  {
    acceleration_ned_m_s2_z = z_ned_m_s2;
    return *this;
  }
 
  TrajectorySetpoint& withYaw(float yaw_rad)
  {
    this->yaw_ned_rad = yaw_rad;
    return *this;
  }
 
  TrajectorySetpoint& withYawRate(float rate_rad_s)
  {
    this->yaw_rate_ned_rad_s = rate_rad_s;
    return *this;
  }
 
  // Helper methods for setting multiple components at once
  TrajectorySetpoint& withPosition(const Eigen::Vector3f& position_ned_m)
  {
    position_ned_m_x = position_ned_m.x();
    position_ned_m_y = position_ned_m.y();
    position_ned_m_z = position_ned_m.z();
    return *this;
  }
 
  TrajectorySetpoint& withHorizontalPosition(const Eigen::Vector2f& position_ned_m)
  {
    position_ned_m_x = position_ned_m.x();
    position_ned_m_y = position_ned_m.y();
    return *this;
  }
 
  TrajectorySetpoint& withVelocity(const Eigen::Vector3f& velocity_ned_m_s)
  {
    velocity_ned_m_s_x = velocity_ned_m_s.x();
    velocity_ned_m_s_y = velocity_ned_m_s.y();
    velocity_ned_m_s_z = velocity_ned_m_s.z();
    return *this;
  }
 
  TrajectorySetpoint& withHorizontalVelocity(const Eigen::Vector2f& velocity_ned_m_s)
  {
    velocity_ned_m_s_x = velocity_ned_m_s.x();
    velocity_ned_m_s_y = velocity_ned_m_s.y();
    return *this;
  }
 
  TrajectorySetpoint& withAcceleration(const Eigen::Vector3f& acceleration_ned_m_s2)
  {
    acceleration_ned_m_s2_x = acceleration_ned_m_s2.x();
    acceleration_ned_m_s2_y = acceleration_ned_m_s2.y();
    acceleration_ned_m_s2_z = acceleration_ned_m_s2.z();
    return *this;
  }
 
  TrajectorySetpoint& withHorizontalAcceleration(const Eigen::Vector2f& acceleration_ned_m_s2)
  {
    acceleration_ned_m_s2_x = acceleration_ned_m_s2.x();
    acceleration_ned_m_s2_y = acceleration_ned_m_s2.y();
    return *this;
  }
};
 
} /* namespace px4_ros2 */