Line data Source code
1 : #ifndef CONTROLS_UTILS_DEBUG_PRINT_H
2 : #define CONTROLS_UTILS_DEBUG_PRINT_H
3 :
4 : #include <algorithm>
5 : #include <iomanip>
6 : #include <sstream>
7 : #include <string>
8 :
9 : #include "utils/Transforms.h"
10 :
11 : constexpr int PRINT_PRECISION = 3;
12 0 : inline const Eigen::IOFormat EigFmt(PRINT_PRECISION, 0, ", ", ";\n", "", "", "[", "]");
13 :
14 : /**
15 : * @brief Formats any Eigen matrix/vector as a string.
16 : * Vectors (single column) are printed transposed as a row.
17 : */
18 : template <typename Derived>
19 1 : std::string fmt_eigen(const Eigen::MatrixBase<Derived>& m) {
20 : std::ostringstream oss;
21 : oss << std::fixed;
22 : if (m.cols() == 1) {
23 : oss << m.transpose().format(EigFmt);
24 : } else {
25 : oss << m.format(EigFmt);
26 : }
27 : return oss.str();
28 : }
29 :
30 : /**
31 : * @brief Formats a Pose as "pos=[x, y, z] rpy=[r, p, y] deg".
32 : */
33 1 : inline std::string fmt_pose(const Pose& pose) {
34 : Vector6d n = pose2n(pose);
35 : n.tail<3>() *= RAD2DEG;
36 : std::ostringstream oss;
37 : oss << std::fixed << std::setprecision(PRINT_PRECISION) << "pos=" << n.head<3>().transpose().format(EigFmt)
38 : << " rpy=" << n.tail<3>().transpose().format(EigFmt) << " deg";
39 : return oss.str();
40 : }
41 :
42 : /**
43 : * @brief Formats a State (pose + twist + acceleration) as a multi-line string.
44 : */
45 1 : inline std::string fmt_state(const State& state) {
46 : Vector6d n = pose2n(state.pose);
47 : n.tail<3>() *= RAD2DEG;
48 : std::ostringstream oss;
49 : oss << std::fixed << std::setprecision(PRINT_PRECISION) << "pose=" << n.transpose().format(EigFmt)
50 : << " (xyz m, rpy deg)\n"
51 : << "twist=" << state.twist.transpose().format(EigFmt) << "\n"
52 : << "accel=" << state.acceleration.transpose().format(EigFmt);
53 : return oss.str();
54 : }
55 :
56 : // ── Box-drawing table helpers ──────────────────────────────────────────
57 :
58 0 : inline void fmt_hline(std::ostringstream& s, int w, const char* l, const char* r) {
59 : s << l;
60 : for (int i = 0; i < w; ++i) s << "\xe2\x94\x80";
61 : s << r << "\n";
62 : }
63 :
64 0 : inline void fmt_row(std::ostringstream& s, int w, const std::string& content) {
65 : int pad = w - static_cast<int>(content.size());
66 : s << "\xe2\x94\x82" << content << std::string(std::max(pad, 0), ' ') << "\xe2\x94\x82\n";
67 : }
68 :
69 0 : inline std::string fmt_f(double v, int w = 7, int prec = 2) {
70 : std::ostringstream s;
71 : s << std::fixed << std::setprecision(prec) << std::setw(w) << v;
72 : return s.str();
73 : }
74 :
75 0 : inline std::string fmt_v3(const char* lbl, double a, double b, double c, const char* tag = "") {
76 : std::ostringstream r;
77 : r << " " << std::setw(14) << std::left << lbl << std::right << "[" << fmt_f(a, 8) << "," << fmt_f(b, 8) << ","
78 : << fmt_f(c, 8) << "]";
79 : if (tag[0]) r << " " << tag;
80 : return r.str();
81 : }
82 :
83 0 : inline std::string fmt_v6(const char* lbl, double a, double b, double c, double d, double e, double g) {
84 : std::ostringstream r;
85 : r << " " << std::setw(10) << std::left << lbl << std::right << "[" << fmt_f(a) << "," << fmt_f(b) << ","
86 : << fmt_f(c) << "," << fmt_f(d) << "," << fmt_f(e) << "," << fmt_f(g) << "]";
87 : return r.str();
88 : }
89 :
90 0 : inline std::string fmt_scalar(const char* lbl, double v) {
91 : std::ostringstream r;
92 : r << " " << std::setw(18) << std::left << lbl << std::right << fmt_f(v, 8);
93 : return r.str();
94 : }
95 :
96 : /**
97 : * @brief Formats a vehicle config struct as a boxed table string.
98 : *
99 : * Templated to avoid a hard dependency on ControlsROS2Wrapper::Params.
100 : */
101 : template <typename Params>
102 1 : std::string fmt_config(const Params& c) {
103 : static constexpr int W = 68;
104 : auto hl = [](std::ostringstream& s, const char* l, const char* r) { fmt_hline(s, W, l, r); };
105 : auto r = [](std::ostringstream& s, const std::string& content) { fmt_row(s, W, content); };
106 :
107 : // clang-format off
108 : std::ostringstream os;
109 : os << "\n";
110 :
111 : hl(os, "\xe2\x94\x8c", "\xe2\x94\x90");
112 : r(os, " Vehicle Configuration");
113 : hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
114 :
115 : r(os, " Dynamics");
116 : r(os, fmt_scalar("mass", c.mass));
117 : r(os, fmt_scalar("buoyancy_factor", c.buoyancy_factor));
118 : r(os, fmt_v3("inertia", c.i_xx, c.i_yy, c.i_zz, "xx yy zz"));
119 : r(os, fmt_v3("", c.i_xy, c.i_xz, c.i_yz, "xy xz yz"));
120 : r(os, fmt_v3("added_mass", c.ma_x, c.ma_y, c.ma_z, "x y z"));
121 : r(os, fmt_v3("", c.ma_roll, c.ma_pitch, c.ma_yaw, "r p w"));
122 : r(os, fmt_v3("lin_drag", c.dl_x, c.dl_y, c.dl_z, "x y z"));
123 : r(os, fmt_v3("", c.dl_roll, c.dl_pitch, c.dl_yaw, "r p w"));
124 : r(os, fmt_v3("nl_drag", c.dnl_x, c.dnl_y, c.dnl_z, "x y z"));
125 : r(os, fmt_v3("", c.dnl_roll, c.dnl_pitch, c.dnl_yaw, "r p w"));
126 : r(os, fmt_v3("r_bb", c.rbb_x, c.rbb_y, c.rbb_z));
127 : r(os, fmt_v3("r_gb", c.rgb_x, c.rgb_y, c.rgb_z));
128 :
129 : hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
130 : r(os, " Controller");
131 : r(os, fmt_scalar("ff", c.ff_gain));
132 : r(os, fmt_scalar("fb", c.fb_gain));
133 : r(os, fmt_scalar("buoyancy", c.buoyancy_gain));
134 : r(os, " scheduling " + std::string(c.surface_gain_scheduling ? "ON" : "OFF") + " (threshold " + fmt_f(c.surface_threshold, 4) + ")");
135 : r(os, "");
136 : r(os, " x y z r p w");
137 : 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));
138 : r(os, fmt_v6("Kp", c.kp_x, c.kp_y, c.kp_z, c.kp_roll, c.kp_pitch, c.kp_yaw));
139 : r(os, fmt_v6("Ki", c.ki_x, c.ki_y, c.ki_z, c.ki_roll, c.ki_pitch, c.ki_yaw));
140 : r(os, fmt_v6("Kd", c.kd_x, c.kd_y, c.kd_z, c.kd_roll, c.kd_pitch, c.kd_yaw));
141 : 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));
142 : 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));
143 : 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));
144 :
145 : hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
146 : r(os, " Trajectory Limits");
147 : r(os, " vel acc jerk");
148 : r(os, fmt_v3("xy", c.max_xy_vel, c.max_xy_acc, c.max_xy_jerk));
149 : r(os, fmt_v3("z", c.max_z_vel, c.max_z_acc, c.max_z_jerk));
150 : r(os, fmt_v3("yaw", c.max_yaw_vel, c.max_yaw_acc, c.max_yaw_jerk));
151 : r(os, fmt_scalar("slow_down_gain", c.slow_down_gain));
152 :
153 : hl(os, "\xe2\x94\x9c", "\xe2\x94\xa4");
154 : r(os, " Thrust Allocation");
155 : r(os, fmt_v3("cg_offset", c.cg_offset_x, c.cg_offset_y, c.cg_offset_z));
156 : hl(os, "\xe2\x94\x94", "\xe2\x94\x98");
157 : // clang-format on
158 :
159 : return os.str();
160 : }
161 :
162 : #endif // CONTROLS_UTILS_DEBUG_PRINT_H
|
