got matrix logging stuff to compile
diff --git a/aos/common/logging/logging_impl.cc b/aos/common/logging/logging_impl.cc
index 681d202..f8312a7 100644
--- a/aos/common/logging/logging_impl.cc
+++ b/aos/common/logging/logging_impl.cc
@@ -122,7 +122,7 @@
fprintf(output, BASE_FORMAT "%.*s", BASE_ARGS,
static_cast<int>(message.message_length), message.message);
break;
- case LogMessage::Type::kStruct:
+ case LogMessage::Type::kStruct: {
char buffer[1024];
size_t output_length = sizeof(buffer);
size_t input_length = message.message_length;
@@ -144,7 +144,30 @@
static_cast<int>(message.structure.string_length),
message.structure.serialized,
static_cast<int>(sizeof(buffer) - output_length), buffer);
- break;
+ } break;
+ case LogMessage::Type::kMatrix: {
+ char buffer[1024];
+ size_t output_length = sizeof(buffer);
+ if (message.message_length !=
+ static_cast<size_t>(message.matrix.rows * message.matrix.cols *
+ MessageType::Sizeof(message.matrix.type))) {
+ LOG(FATAL, "expected %d bytes of matrix data but have %zu\n",
+ message.matrix.rows * message.matrix.cols *
+ MessageType::Sizeof(message.matrix.type),
+ message.message_length);
+ }
+ if (!PrintMatrix(buffer, &output_length,
+ message.matrix.data + message.matrix.string_length,
+ message.matrix.type, message.matrix.rows,
+ message.matrix.cols)) {
+ LOG(FATAL, "printing %dx%d matrix of type %" PRIu32 " failed\n",
+ message.matrix.rows, message.matrix.cols, message.matrix.type);
+ }
+ fprintf(output, BASE_FORMAT "%.*s: %.*s\n", BASE_ARGS,
+ static_cast<int>(message.matrix.string_length),
+ message.structure.serialized,
+ static_cast<int>(sizeof(buffer) - output_length), buffer);
+ } break;
}
#undef NSECONDS_DIGITS
#undef BASE_FORMAT
@@ -174,6 +197,28 @@
static_cast<int>(sizeof(printed) - printed_bytes), printed);
}
+void LogImplementation::LogMatrix(
+ log_level level, const ::std::string &message, uint32_t type_id,
+ int rows, int cols, const void *data) {
+ char serialized[1024];
+ if (static_cast<size_t>(rows * cols * MessageType::Sizeof(type_id)) >
+ sizeof(serialized)) {
+ LOG(FATAL, "matrix of size %u too big to serialize\n",
+ rows * cols * MessageType::Sizeof(type_id));
+ }
+ SerializeMatrix(type_id, serialized, data, rows, cols);
+ char printed[1024];
+ size_t printed_bytes = sizeof(printed);
+ if (!PrintMatrix(printed, &printed_bytes, serialized, type_id, rows, cols)) {
+ LOG(FATAL, "PrintMatrix(%p, %p(=%zd), %p, %" PRIu32 ", %d, %d) failed\n",
+ printed, &printed_bytes, printed_bytes, serialized, type_id, rows,
+ cols);
+ }
+ DoLogVariadic(level, "%.*s: %.*s\n", static_cast<int>(message.size()),
+ message.data(),
+ static_cast<int>(sizeof(printed) - printed_bytes), printed);
+}
+
StreamLogImplementation::StreamLogImplementation(FILE *stream)
: stream_(stream) {}
diff --git a/aos/common/logging/logging_impl.h b/aos/common/logging/logging_impl.h
index 11b28b0..1d68a16 100644
--- a/aos/common/logging/logging_impl.h
+++ b/aos/common/logging/logging_impl.h
@@ -46,7 +46,8 @@
};
int32_t seconds, nseconds;
- // message_length is the length of everything in message for all types.
+ // message_length is just the length of the actual data (which member depends
+ // on the type).
size_t message_length, name_length;
pid_t source;
static_assert(sizeof(source) == 4, "that's how they get printed");
@@ -66,9 +67,11 @@
struct {
// The type ID of the element type.
uint32_t type;
- int rows, columns;
+ int rows, cols;
+ size_t string_length;
+ // The message string and then the serialized matrix.
char
- data[LOG_MESSAGE_LEN - sizeof(type) - sizeof(rows) - sizeof(columns)];
+ data[LOG_MESSAGE_LEN - sizeof(type) - sizeof(rows) - sizeof(cols)];
} matrix;
};
};
@@ -170,9 +173,10 @@
size_t size, const MessageType *type,
const ::std::function<size_t(char *)> &serialize,
int levels);
+ // This one is implemented in matrix_logging.cc.
static void DoLogMatrix(log_level level, const ::std::string &message,
uint32_t type_id, int rows, int cols,
- const void *data);
+ const void *data, int levels);
// Friends so that they can access the static Do* functions.
friend void VLog(log_level, const char *, va_list);
diff --git a/aos/common/logging/matrix_logging-tmpl.h b/aos/common/logging/matrix_logging-tmpl.h
index 8d18f67..bfc3ce5 100644
--- a/aos/common/logging/matrix_logging-tmpl.h
+++ b/aos/common/logging/matrix_logging-tmpl.h
@@ -10,8 +10,10 @@
template <class T>
void DoLogMatrix(log_level level, const ::std::string &message,
const T &matrix) {
+ static_assert(!T::IsRowMajor, "we only handle column-major storage");
LogImplementation::DoLogMatrix(level, message, TypeID<typename T::Scalar>::id,
- matrix.rows(), matrix.cols(), matrix.data());
+ matrix.rows(), matrix.cols(), matrix.data(),
+ 1);
}
} // namespace logging
diff --git a/aos/common/logging/matrix_logging.cc b/aos/common/logging/matrix_logging.cc
index cd30509..6173074 100644
--- a/aos/common/logging/matrix_logging.cc
+++ b/aos/common/logging/matrix_logging.cc
@@ -1 +1,35 @@
#include "aos/common/logging/matrix_logging.h"
+
+#include "aos/common/queue_types.h"
+
+namespace aos {
+namespace logging {
+
+void LogImplementation::DoLogMatrix(log_level level,
+ const ::std::string &message,
+ uint32_t type_id, int rows, int cols,
+ const void *data, int levels) {
+ internal::RunWithCurrentImplementation(
+ levels, [&](LogImplementation * implementation) {
+ implementation->LogMatrix(level, message, type_id, rows, cols, data);
+ });
+
+ if (level == FATAL) {
+ char serialized[1024];
+ if (static_cast<size_t>(rows * cols * MessageType::Sizeof(type_id)) >
+ sizeof(serialized)) {
+ Die("LOG(FATAL) matrix too big to serialize");
+ }
+ SerializeMatrix(type_id, serialized, data, rows, cols);
+ char printed[LOG_MESSAGE_LEN];
+ size_t printed_bytes = sizeof(printed);
+ if (!PrintMatrix(printed, &printed_bytes, serialized, type_id, rows, cols)) {
+ Die("LOG(FATAL) PrintMatrix call failed");
+ }
+ Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
+ static_cast<int>(printed_bytes), printed);
+ }
+}
+
+} // namespace logging
+} // namespace aos
diff --git a/aos/common/logging/queue_logging.cc b/aos/common/logging/queue_logging.cc
index 021da7c..ce96c29 100644
--- a/aos/common/logging/queue_logging.cc
+++ b/aos/common/logging/queue_logging.cc
@@ -13,22 +13,22 @@
internal::RunWithCurrentImplementation(
levels, [&](LogImplementation * implementation) {
implementation->LogStruct(level, message, size, type, serialize);
-
- if (level == FATAL) {
- char serialized[1024];
- if (size > sizeof(serialize)) {
- Die("LOG(FATAL) structure too big to serialize");
- }
- size_t used = serialize(serialized);
- char printed[LOG_MESSAGE_LEN];
- size_t printed_bytes = sizeof(printed);
- if (!PrintMessage(printed, &printed_bytes, serialized, &used, *type)) {
- Die("LOG(FATAL) PrintMessage call failed");
- }
- Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
- static_cast<int>(printed_bytes), printed);
- }
});
+
+ if (level == FATAL) {
+ char serialized[1024];
+ if (size > sizeof(serialized)) {
+ Die("LOG(FATAL) structure too big to serialize");
+ }
+ size_t used = serialize(serialized);
+ char printed[LOG_MESSAGE_LEN];
+ size_t printed_bytes = sizeof(printed);
+ if (!PrintMessage(printed, &printed_bytes, serialized, &used, *type)) {
+ Die("LOG(FATAL) PrintMessage call failed");
+ }
+ Die("%.*s: %.*s\n", static_cast<int>(message.size()), message.data(),
+ static_cast<int>(printed_bytes), printed);
+ }
}
} // namespace logging
diff --git a/aos/common/queue_types.cc b/aos/common/queue_types.cc
index 22b6f4a..93cf135 100644
--- a/aos/common/queue_types.cc
+++ b/aos/common/queue_types.cc
@@ -164,6 +164,95 @@
return true;
}
+bool PrintMatrix(char *output, size_t *output_bytes, const void *input,
+ uint32_t type_id, int rows, int cols) {
+ CHECK(MessageType::IsPrimitive(type_id));
+ const size_t element_size = MessageType::Sizeof(type_id);
+
+ if (*output_bytes < 1) return false;
+ *output_bytes -= 1;
+ *(output++) = '[';
+
+ bool first_row = true;
+ for (int row = 0; row < rows; ++row) {
+ if (first_row) {
+ first_row = false;
+ } else {
+ if (*output_bytes < 2) return false;
+ *output_bytes -= 2;
+ *(output++) = ',';
+ *(output++) = ' ';
+ }
+
+ if (*output_bytes < 1) return false;
+ *output_bytes -= 1;
+ *(output++) = '[';
+
+ bool first_col = true;
+ for (int col = 0; col < cols; ++col) {
+ if (first_col) {
+ first_col = false;
+ } else {
+ if (*output_bytes < 2) return false;
+ *output_bytes -= 2;
+ *(output++) = ',';
+ *(output++) = ' ';
+ }
+
+ const size_t output_bytes_before = *output_bytes;
+ size_t input_bytes = element_size;
+ if (!PrintField(output, output_bytes,
+ static_cast<const char *>(input) +
+ (row + col * rows) * element_size,
+ &input_bytes, type_id)) {
+ return false;
+ }
+ CHECK_EQ(0u, input_bytes);
+ // Update the output pointer, ignoring the trailing '\0' that
+ // the subcall put on.
+ output += output_bytes_before - *output_bytes - 1;
+ *output_bytes += 1;
+ }
+
+ if (*output_bytes < 1) return false;
+ *output_bytes -= 1;
+ *(output++) = ']';
+ }
+ if (*output_bytes < 2) return false;
+ *output_bytes -= 2;
+ *(output++) = ']';
+ *(output++) = '\0';
+ return true;
+}
+
+void SerializeMatrix(int type_id, void *output_void, const void *input_void,
+ int rows, int cols) {
+ char *const output = static_cast<char *>(output_void);
+ const char *const input = static_cast<const char *>(input_void);
+
+ CHECK(MessageType::IsPrimitive(type_id));
+ const size_t element_size = MessageType::Sizeof(type_id);
+
+ for (int i = 0; i < rows * cols; ++i) {
+ switch(element_size) {
+ case 1:
+ to_network<1>(&input[i * element_size], &output[i * element_size]);
+ break;
+ case 2:
+ to_network<2>(&input[i * element_size], &output[i * element_size]);
+ break;
+ case 4:
+ to_network<4>(&input[i * element_size], &output[i * element_size]);
+ break;
+ case 8:
+ to_network<8>(&input[i * element_size], &output[i * element_size]);
+ break;
+ default:
+ LOG(FATAL, "illegal primitive type size %zu\n", element_size);
+ }
+ }
+}
+
namespace type_cache {
namespace {
diff --git a/aos/common/queue_types.h b/aos/common/queue_types.h
index a1d9ce0..b95c946 100644
--- a/aos/common/queue_types.h
+++ b/aos/common/queue_types.h
@@ -9,6 +9,7 @@
#include <string>
#include "aos/common/macros.h"
+#include "aos/common/byteorder.h"
namespace aos {
@@ -52,6 +53,14 @@
return (type_id & 0x2000) != 0;
}
+ static unsigned int Sizeof(uint32_t type_id) {
+ if (IsPrimitive(type_id)) {
+ return (type_id & 0xFFFF) - 0x2000;
+ } else {
+ return type_id & 0xFFFF;
+ }
+ }
+
// How many (serialized) bytes the superclass takes up.
uint16_t super_size;
// The type ID for this.
@@ -101,6 +110,14 @@
bool PrintMessage(char *output, size_t *output_bytes, const void *input,
size_t *input_bytes, const MessageType &type)
__attribute__((warn_unused_result));
+// Calls PrintField to print out a matrix of values.
+bool PrintMatrix(char *output, size_t *output_bytes, const void *input,
+ uint32_t type, int rows, int cols);
+
+// "Serializes" a matrix (basically just converts to network byte order). The
+// result can be passed to PrintMatrix.
+void SerializeMatrix(int type_id, void *output_void, const void *input_void,
+ int rows, int cols);
// Implements a cache of types which generally works per-process but can (when
// instructed) put a type in shared memory which other processes will
diff --git a/aos/common/queue_types_test.cc b/aos/common/queue_types_test.cc
index 72eae94..ac7e683 100644
--- a/aos/common/queue_types_test.cc
+++ b/aos/common/queue_types_test.cc
@@ -6,6 +6,7 @@
#include "aos/common/test_queue.q.h"
#include "aos/common/byteorder.h"
+#include "aos/queue_primitives.h"
using ::aos::common::testing::Structure;
using ::aos::common::testing::MessageWithStructure;
@@ -174,5 +175,19 @@
EXPECT_EQ(kTestStructure1String.size() + 1, sizeof(output) - output_bytes);
}
+TEST_F(PrintMessageTest, Matrix) {
+ static const uint16_t kTestMatrix[] = {971, 254, 1768, 8971, 9971, 973};
+ uint16_t test_matrix[sizeof(kTestMatrix) / sizeof(kTestMatrix[0])];
+ SerializeMatrix(queue_primitive_types::uint16_t_p, test_matrix, kTestMatrix,
+ 3, 2);
+ static const ::std::string kOutput =
+ "[[971, 8971], [254, 9971], [1768, 973]]";
+ output_bytes = sizeof(output);
+ ASSERT_TRUE(PrintMatrix(output, &output_bytes, test_matrix,
+ queue_primitive_types::uint16_t_p, 3, 2));
+ EXPECT_EQ(kOutput, ::std::string(output));
+ EXPECT_EQ(kOutput.size() + 1, sizeof(output) - output_bytes);
+}
+
} // namespace testing
} // namespace aos