Print sendtime of messsages.
Change-Id: I42579bce6133a536df61da9355aed763a0b2e614
diff --git a/aos/build/queues/output/message_dec.rb b/aos/build/queues/output/message_dec.rb
index 20a0c9d..0f5694a 100644
--- a/aos/build/queues/output/message_dec.rb
+++ b/aos/build/queues/output/message_dec.rb
@@ -335,7 +335,7 @@
args.push(["AOS_TIME_ARGS(static_cast<int32_t>(",
"::std::chrono::duration_cast<::std::chrono::seconds>(",
"#{parent}#{this_name}.time_since_epoch()).count()), ",
- "static_cast<int32_t>(::std::chrono::duration_cast<::std::chrono::nanoseconds>(",
+ "static_cast<uint32_t>(::std::chrono::duration_cast<::std::chrono::nanoseconds>(",
"#{parent}#{this_name}.time_since_epoch() - ",
"::std::chrono::duration_cast<::std::chrono::seconds>(",
"#{parent}#{this_name}.time_since_epoch())).count()))"].join(''))
diff --git a/aos/build/queues/print_field.rb b/aos/build/queues/print_field.rb
index 0e27bf0..65837ee 100644
--- a/aos/build/queues/print_field.rb
+++ b/aos/build/queues/print_field.rb
@@ -1,7 +1,8 @@
require_relative 'load.rb'
# TODO(brians): Special-case Time too and float/double if we can find a good way to do it.
-GenericTypeNames = ['float', 'double', 'char', '::aos::monotonic_clock::time_point']
+GenericTypeNames = ['float', 'double', 'char']
+TimeTypeNames = ['::aos::monotonic_clock::time_point']
IntegerSizes = [8, 16, 32, 64]
WriteIffChanged.open(ARGV[0]) do |output|
@@ -47,6 +48,32 @@
}
END2
end.join('')}
+#{TimeTypeNames.collect do |name|
+ message_element = Target::MessageElement.new(name, 'value')
+ statement = MessageElementStmt.new(name, 'value')
+ message_element.size = statement.size
+ print_args = []
+ message_element.fetchPrintArgs(print_args)
+ next <<END2
+ case #{message_element.getTypeID()}:
+ {
+ if (*input_bytes < #{statement.size}) return false;
+ *input_bytes -= #{statement.size};
+ int32_t upper;
+ uint32_t lower;
+ to_host(static_cast<const char *>(input), &upper);
+ to_host(static_cast<const char *>(input) + 4, &lower);
+ #{name} value(::std::chrono::seconds(upper) + ::std::chrono::nanoseconds(lower));
+ int ret = snprintf(output, *output_bytes,
+ "#{statement.toPrintFormat()}",
+ #{print_args[0]});
+ if (ret < 0) return false;
+ if (static_cast<unsigned int>(ret) >= *output_bytes) return false;
+ *output_bytes -= ret;
+ return true;
+ }
+END2
+end.join('')}
#{IntegerSizes.collect do |size|
[true, false].collect do |signed|
size_bytes = size / 8
diff --git a/aos/common/controls/BUILD b/aos/common/controls/BUILD
index de100bb..2e7af21 100644
--- a/aos/common/controls/BUILD
+++ b/aos/common/controls/BUILD
@@ -78,12 +78,12 @@
'control_loop.h',
],
deps = [
- '//aos/common/messages:robot_state',
- '//aos/common/logging',
- '//aos/common:time',
':control_loop_queues',
+ '//aos/common/logging',
'//aos/common/logging:queue_logging',
+ '//aos/common/messages:robot_state',
'//aos/common/util:log_interval',
'//aos/common:queues',
+ '//aos/common:time',
],
)
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index d6813fb..b2f85e7 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -2,8 +2,8 @@
#include <inttypes.h>
#include "aos/common/logging/logging.h"
-#include "aos/common/messages/robot_state.q.h"
#include "aos/common/logging/queue_logging.h"
+#include "aos/common/messages/robot_state.q.h"
namespace aos {
namespace controls {
@@ -88,6 +88,7 @@
control_loop_->output.MakeMessage();
RunIteration(goal, position, output.get(), status.get());
+ output->SetTimeToNow();
LOG_STRUCT(DEBUG, "output", *output);
output.Send();
} else {
@@ -96,6 +97,7 @@
ZeroOutputs();
}
+ status->SetTimeToNow();
LOG_STRUCT(DEBUG, "status", *status);
status.Send();
}
diff --git a/aos/common/message.cc b/aos/common/message.cc
index 818c945..18acaa9 100644
--- a/aos/common/message.cc
+++ b/aos/common/message.cc
@@ -13,7 +13,7 @@
size_t Message::Deserialize(const char *buffer) {
int32_t sec;
- int32_t nsec;
+ uint32_t nsec;
to_host(&buffer[0], &sec);
to_host(&buffer[4], &nsec);
sent_time = monotonic_clock::time_point(chrono::seconds(sec) +
@@ -26,9 +26,9 @@
int32_t sec =
chrono::duration_cast<chrono::seconds>(sent_time.time_since_epoch())
.count();
- int32_t nsec = chrono::duration_cast<chrono::nanoseconds>(
- sent_time.time_since_epoch() - chrono::seconds(sec))
- .count();
+ uint32_t nsec = chrono::duration_cast<chrono::nanoseconds>(
+ sent_time.time_since_epoch() - chrono::seconds(sec))
+ .count();
to_network(&sec, &buffer[0]);
to_network(&nsec, &buffer[4]);
return Size();
@@ -38,10 +38,10 @@
int32_t sec =
chrono::duration_cast<chrono::seconds>(sent_time.time_since_epoch())
.count();
- int32_t nsec = chrono::duration_cast<chrono::nanoseconds>(
- sent_time.time_since_epoch() - chrono::seconds(sec))
- .count();
- return snprintf(buffer, length, "%" PRId32 ".%09" PRId32 "s", sec, nsec);
+ uint32_t nsec = chrono::duration_cast<chrono::nanoseconds>(
+ sent_time.time_since_epoch() - chrono::seconds(sec))
+ .count();
+ return snprintf(buffer, length, "%" PRId32 ".%09" PRIu32 "s", sec, nsec);
}
} // namespace aos
diff --git a/aos/common/message.h b/aos/common/message.h
index 8ae7f16..f46969f 100644
--- a/aos/common/message.h
+++ b/aos/common/message.h
@@ -28,7 +28,11 @@
size_t Serialize(char *buffer) const;
// Populates sent_time with the current time.
- void SetTimeToNow() { sent_time = monotonic_clock::now(); }
+ void SetTimeToNow() {
+ if (sent_time == monotonic_clock::min_time) {
+ sent_time = monotonic_clock::now();
+ }
+ }
// Writes the contents of the message to the provided buffer.
size_t Print(char *buffer, int length) const;
diff --git a/aos/common/queue_types.cc b/aos/common/queue_types.cc
index cff3c4f..3945395 100644
--- a/aos/common/queue_types.cc
+++ b/aos/common/queue_types.cc
@@ -164,16 +164,35 @@
bool PrintMessage(char *output, size_t *output_bytes, const void *input,
size_t *input_bytes, const MessageType &type) {
- *input_bytes -= type.super_size;
- input = static_cast<const char *>(input) + type.super_size;
-
if (*output_bytes < type.name.size() + 1) return false;
*output_bytes -= type.name.size() + 1;
memcpy(output, type.name.data(), type.name.size());
output += type.name.size();
- *(output++) = '{';
+ *output = '{';
+ ++output;
bool first = true;
+ if (type.super_size == 8) {
+ size_t start_input_bytes = *input_bytes;
+
+ if (*output_bytes < 10) return false;
+ memcpy(output, "sendtime: ", 10);
+ output += 10;
+ *output_bytes -= 10;
+
+ size_t output_bytes_before = *output_bytes;
+ if (!PrintField(output, output_bytes, input, input_bytes, 0x5f372008u)) {
+ return false;
+ }
+ output += output_bytes_before - *output_bytes;
+ input = static_cast<const char *>(input) + type.super_size;
+ first = false;
+ CHECK(start_input_bytes == (*input_bytes + 8));
+ } else {
+ *input_bytes -= type.super_size;
+ input = static_cast<const char *>(input) + type.super_size;
+ }
+
for (int i = 0; i < type.number_fields; ++i) {
if (first) {
first = false;
diff --git a/aos/common/queue_types_test.cc b/aos/common/queue_types_test.cc
index bbadf34..96d02bc 100644
--- a/aos/common/queue_types_test.cc
+++ b/aos/common/queue_types_test.cc
@@ -89,7 +89,7 @@
class PrintMessageTest : public ::testing::Test {
public:
- char input[128], output[256];
+ char input[512], output[512];
size_t input_bytes, output_bytes;
};
@@ -194,18 +194,18 @@
static const OtherTestingMessage kTestMessage1(true, 8971, 3.2);
static const ::std::string kTestMessage1String =
- ".aos.common.testing.OtherTestingMessage{test_bool:T, test_int:8971"
+ ".aos.common.testing.OtherTestingMessage{sendtime: -633437440.3735158s, test_bool:T, test_int:8971"
", test_double:3.200000}";
static const Structure kTestStructure1(false, 973, 8.56);
static const ::std::string kTestStructure1String =
- ".aos.common.testing.Structure{struct_bool:f, struct_int:973"
- ", struct_float:8.560000}";
+ ".aos.common.testing.Structure{struct_bool:f, struct_int:973, "
+ "struct_float:8.560000}";
static const ::aos::common::testing::Structure kStructureValue(true, 973, 3.14);
static const MessageWithArrays kTestMessageWithArrays({{971, 254, 1678}},
{{kStructureValue,
kStructureValue}});
static const ::std::string kTestMessageWithArraysString =
- ".aos.common.testing.MessageWithArrays{test_int:[971, 254, 1678], "
+ ".aos.common.testing.MessageWithArrays{sendtime: -633437440.3735158s, test_int:[971, 254, 1678], "
"test_struct:[.aos.common.testing.Structure{struct_bool:T, struct_int:973, "
"struct_float:3.140000}, .aos.common.testing.Structure{struct_bool:T, "
"struct_int:973, struct_float:3.140000}]}";