feat: implementation of a pid dp controller

[Talhanc]

DP controller with quaternion representation used.
The controller is tuned and works in the simulator

Added in docstring for the functions in .hpp file
ROS2 functions are defined separate from the normal functions ( cough Christopher asked for this cough )

Source: Trust me bro

[Andeshog]

Also; write a README.md for both the pid package and ref. filter package

[Andeshog]

consider using a different notation for quat, since having two sets of xyz in the same vector can be misleading/confusing.

[chrstrom]

Also see my comment relating to quaternion operations, Eigen::Quaterniond exists for a good reason 🔥

[chrstrom]

First review pass here, I think I'll stop by the office tomorrow if you have any other questions 🫡 (There are also a few choices here relating to data structures and how the code is laid out that I think it's easier to discuss in-person)

[chrstrom]

Note that this can get really annoying if you want to add more parameters. There is support for dynamic setting of rosparams that doesnt rely on topics, but i actually think subscribers are nicer in this specific case. IF (and only if) you need to add more subscribers here, this way of doing it requires some annoying boilerplate. In that case, you might want to make your life a bit easier by defining some helpers, like:

template <typename MessageType>
void add_subscriber(const std::string &topic, void (PIDControllerNode::*callback)(const std::shared_ptr<MessageType>)) {
    constexpr std::size_t queue_size = 10;
    subscriptions_.emplace_back(
        this->create_subscription<MessageType>(
            topic, queue_size , [this, callback](const std::shared_ptr<MessageType> msg) {
                (this->*callback)(msg);
            }));
}

that adds to the member variable

std::vector<rclcpp::SubscriptionBase::SharedPtr> subscriptions_

which lets you add your subscriptions like

subscriptions_ = {
    add_subscriber<nav_msgs::msg::Odometry>("/nucleus/odom", &PIDControllerNode::odometry_callback),
    add_subscriber<vortex_msgs::msg::ReferenceFilter>("/dp/reference", &PIDControllerNode::guidance_callback),
    add_subscriber<std_msgs::msg::Float64MultiArray>("/pid/kp", &PIDControllerNode::kp_callback),
    add_subscriber<std_msgs::msg::Float64MultiArray>("/pid/ki", &PIDControllerNode::ki_callback),
    add_subscriber<std_msgs::msg::Float64MultiArray>("/pid/kd", &PIDControllerNode::kd_callback)
};

[chrstrom]

This should raise a red flag, specifically the choice of maintaining quaternions as part of a larger Eigen:: vector, or as an Eigen::Vector4d. I suggest tracing back through the code and using Eigen::Quaterniond directly (for example, you can just use * between two Eigen quats and it does quaternion multiplication for you).

If you absolutely need the quaternion to be part of the Eigen::Vector7d type, do the conversion right before you need it.

Something like

struct Eta
{
    Eigen::Vector3d pos;
    Eigen::Quaterniond rot;
}

could maybe make sense?

[chrstrom]

This method is fairly unwieldy, you should consider adding a few free functions to make it more manageable. Also make sure to document how this call is blocking, and what conditions apply wrt that

[chrstrom]

Writing out the equations in full makes it really hard to see which convention is used, might want to just rely on Eigen:: builtins:

Eigen::Matrix3d calculate_r(const Eigen::Vector6d& eta) {
    const double roll = eta(3); 
    const double pitch = eta(4); 
    const double yaw = eta(5);

    const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
    const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
    const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());

    const Eigen::Quaterniond q = yaw_angle * pitch_angle * roll_angle;

    return q.toRotationMatrix();
}

[chrstrom]

Would it be possible to follow a similar pattern to my suggestion above?

[chrstrom]

As far as I can see, this is never caught?

Either catch it and handle it appropriately, or crash and die

[Talhanc]

I thought maybe this works better, what do you think? Maybe still change the transformation matrix?

try {
        if (ctheta == 0) {
            throw std::runtime_error("Division by zero in calculate_T.");
        } else {
            transformation_matrix << 1, sphi * stheta / ctheta, cphi * stheta / ctheta, 0, cphi, -sphi, 0, sphi / ctheta, cphi / ctheta;
        }
    } catch (std::runtime_error& e) {
        transformation_matrix = Eigen::Matrix3d::Identity();
    }