DebugPrint.h

Go to the documentation of this file.

#ifndef CONTROLS_UTILS_DEBUG_PRINT_H
#define CONTROLS_UTILS_DEBUG_PRINT_H
 
#include <algorithm>
#include <iomanip>
#include <sstream>
#include <string>
 
#include "utils/Transforms.h"
 
constexpr int PRINT_PRECISION = 3;
inline const Eigen::IOFormat EigFmt(PRINT_PRECISION, 0, ", ", ";\n", "", "", "[", "]");
 
template <typename Derived>
std::string fmt_eigen(const Eigen::MatrixBase<Derived>& m) {
    std::ostringstream oss;
    oss << std::fixed;
    if (m.cols() == 1) {
        oss << m.transpose().format(EigFmt);
    } else {
        oss << m.format(EigFmt);
    }
    return oss.str();
}
 
inline std::string fmt_pose(const Pose& pose) {
    Vector6d n = pose2n(pose);
    n.tail<3>() *= RAD2DEG;
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(PRINT_PRECISION) << "pos=" << n.head<3>().transpose().format(EigFmt)
        << " rpy=" << n.tail<3>().transpose().format(EigFmt) << " deg";
    return oss.str();
}
 
inline std::string fmt_state(const State& state) {
    Vector6d n = pose2n(state.pose);
    n.tail<3>() *= RAD2DEG;
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(PRINT_PRECISION) << "pose=" << n.transpose().format(EigFmt)
        << " (xyz m, rpy deg)\n"
        << "twist=" << state.twist.transpose().format(EigFmt) << "\n"
        << "accel=" << state.acceleration.transpose().format(EigFmt);
    return oss.str();
}
 
// ── Box-drawing table helpers ──────────────────────────────────────────
 
inline void fmt_hline(std::ostringstream& s, int w, const char* l, const char* r) {
    s << l;
    for (int i = 0; i < w; ++i) s << "\xe2\x94\x80";
    s << r << "\n";
}
 
inline void fmt_row(std::ostringstream& s, int w, const std::string& content) {
    int pad = w - static_cast<int>(content.size());
    s << "\xe2\x94\x82" << content << std::string(std::max(pad, 0), ' ') << "\xe2\x94\x82\n";
}
 
inline std::string fmt_f(double v, int w = 7, int prec = 2) {
    std::ostringstream s;
    s << std::fixed << std::setprecision(prec) << std::setw(w) << v;
    return s.str();
}
 
inline std::string fmt_v3(const char* lbl, double a, double b, double c, const char* tag = "") {
    std::ostringstream r;
    r << "    " << std::setw(14) << std::left << lbl << std::right << "[" << fmt_f(a, 8) << "," << fmt_f(b, 8) << ","
      << fmt_f(c, 8) << "]";
    if (tag[0]) r << "  " << tag;
    return r.str();
}
 
inline std::string fmt_v6(const char* lbl, double a, double b, double c, double d, double e, double g) {
    std::ostringstream r;
    r << "    " << std::setw(10) << std::left << lbl << std::right << "[" << fmt_f(a) << "," << fmt_f(b) << ","
      << fmt_f(c) << "," << fmt_f(d) << "," << fmt_f(e) << "," << fmt_f(g) << "]";
    return r.str();
}
 
inline std::string fmt_scalar(const char* lbl, double v) {
    std::ostringstream r;
    r << "    " << std::setw(18) << std::left << lbl << std::right << fmt_f(v, 8);
    return r.str();
}
 
template <typename Params>
std::string fmt_config(const Params& c) {
    static constexpr int W = 68;
    auto hl                = [](std::ostringstream& s, const char* l, const char* r) { fmt_hline(s, W, l, r); };
    auto r                 = [](std::ostringstream& s, const std::string& content) { fmt_row(s, W, content); };
 
    // clang-format off
    std::ostringstream os;
    os << "\n";
 
    hl(os, "\xe2\x94\x8c", "\xe2\x94\x90");
    r(os, "  Vehicle Configuration");
    hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
 
    r(os, "  Dynamics");
    r(os, fmt_scalar("mass", c.mass));
    r(os, fmt_scalar("buoyancy_factor", c.buoyancy_factor));
    r(os, fmt_v3("inertia",    c.i_xx,     c.i_yy,      c.i_zz,     "xx yy zz"));
    r(os, fmt_v3("",           c.i_xy,     c.i_xz,      c.i_yz,     "xy xz yz"));
    r(os, fmt_v3("added_mass", c.ma_x,     c.ma_y,      c.ma_z,     "x  y  z"));
    r(os, fmt_v3("",           c.ma_roll,  c.ma_pitch,   c.ma_yaw,   "r  p  w"));
    r(os, fmt_v3("lin_drag",   c.dl_x,     c.dl_y,      c.dl_z,     "x  y  z"));
    r(os, fmt_v3("",           c.dl_roll,  c.dl_pitch,   c.dl_yaw,   "r  p  w"));
    r(os, fmt_v3("nl_drag",    c.dnl_x,    c.dnl_y,     c.dnl_z,    "x  y  z"));
    r(os, fmt_v3("",           c.dnl_roll, c.dnl_pitch,  c.dnl_yaw,  "r  p  w"));
    r(os, fmt_v3("r_bb",       c.rbb_x,    c.rbb_y,     c.rbb_z));
    r(os, fmt_v3("r_gb",       c.rgb_x,    c.rgb_y,     c.rgb_z));
 
    hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
    r(os, "  Controller");
    r(os, fmt_scalar("ff", c.ff_gain));
    r(os, fmt_scalar("fb", c.fb_gain));
    r(os, fmt_scalar("buoyancy", c.buoyancy_gain));
    r(os, "    scheduling        " + std::string(c.surface_gain_scheduling ? "ON" : "OFF") + "  (threshold " + fmt_f(c.surface_threshold, 4) + ")");
    r(os, "");
    r(os, "                       x      y      z      r      p      w");
    r(os, fmt_v6("ax_gain", c.axis_gain_x, c.axis_gain_y, c.axis_gain_z, c.axis_gain_roll, c.axis_gain_pitch, c.axis_gain_yaw));
    r(os, fmt_v6("Kp",      c.kp_x,       c.kp_y,       c.kp_z,       c.kp_roll,       c.kp_pitch,       c.kp_yaw));
    r(os, fmt_v6("Ki",      c.ki_x,       c.ki_y,       c.ki_z,       c.ki_roll,       c.ki_pitch,       c.ki_yaw));
    r(os, fmt_v6("Kd",      c.kd_x,       c.kd_y,       c.kd_z,       c.kd_roll,       c.kd_pitch,       c.kd_yaw));
    r(os, fmt_v6("Kp_sf",   c.kp_x_surface, c.kp_y_surface, c.kp_z_surface, c.kp_roll_surface, c.kp_pitch_surface, c.kp_yaw_surface));
    r(os, fmt_v6("Ki_sf",   c.ki_x_surface, c.ki_y_surface, c.ki_z_surface, c.ki_roll_surface, c.ki_pitch_surface, c.ki_yaw_surface));
    r(os, fmt_v6("Kd_sf",   c.kd_x_surface, c.kd_y_surface, c.kd_z_surface, c.kd_roll_surface, c.kd_pitch_surface, c.kd_yaw_surface));
 
    hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
    r(os, "  Trajectory Limits");
    r(os, "                       vel      acc     jerk");
    r(os, fmt_v3("xy",  c.max_xy_vel,  c.max_xy_acc,  c.max_xy_jerk));
    r(os, fmt_v3("z",   c.max_z_vel,   c.max_z_acc,   c.max_z_jerk));
    r(os, fmt_v3("yaw", c.max_yaw_vel, c.max_yaw_acc, c.max_yaw_jerk));
    r(os, fmt_scalar("slow_down_gain", c.slow_down_gain));
 
    hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
    r(os, "  Thrust Allocation");
    r(os, fmt_v3("cg_offset", c.cg_offset_x, c.cg_offset_y, c.cg_offset_z));
    hl(os, "\xe2\x94\x94", "\xe2\x94\x98");
    // clang-format on
 
    return os.str();
}
 
#endif  // CONTROLS_UTILS_DEBUG_PRINT_H