trajectory.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <px4_msgs/msg/trajectory_setpoint.hpp>
#include <Eigen/Eigen>
 
#include <px4_ros2/common/setpoint_base.hpp>
 
namespace px4_ros2
{
struct TrajectorySetpoint;
 
class TrajectorySetpointType : public SetpointBase
{
public:
  explicit TrajectorySetpointType(Context & context);
 
  ~TrajectorySetpointType() override = default;
 
  Configuration getConfiguration() 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;
};
 
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 */