Modified can parser to run it from PlotJuggler (#352)

* Modified can parser to run it from PlotJuggler

* Assert for parsing DynamicStruct, can ignore checksum/counter messages

* Only write processed dbc on changes, to prevent cmake compiling

* Do not fail on creating new DBC outputs

* Remove print statement

* outer ignore_checksum and ignore_counter

* whitespace

can/common.h

@@ -5,8 +5,12 @@
 #include <unordered_map>
 
 #include "common_dbc.h"
+#include <capnp/dynamic.h>
 #include <capnp/serialize.h>
+
+#ifndef DYNAMIC_CAPNP
 #include "cereal/gen/cpp/log.capnp.h"
+#endif
 
 #define MAX_BAD_COUNTER 5
 
@@ -36,6 +40,9 @@ public:
   uint8_t counter;
   uint8_t counter_fail;
 
+  bool ignore_checksum = false;
+  bool ignore_counter = false;
+
   bool parse(uint64_t sec, uint16_t ts_, uint8_t * dat);
   bool update_counter_generic(int64_t v, int cnt_size);
 };
@@ -55,9 +62,13 @@ public:
   CANParser(int abus, const std::string& dbc_name,
             const std::vector<MessageParseOptions> &options,
             const std::vector<SignalParseOptions> &sigoptions);
-  void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans);
+  CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter);
-  void UpdateValid(uint64_t sec);
+  #ifndef DYNAMIC_CAPNP
   void update_string(const std::string &data, bool sendcan);
+  void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans);
+  #endif
+  void UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cans);
+  void UpdateValid(uint64_t sec);
   std::vector<SignalValue> query_latest();
 };
 
@@ -69,5 +80,6 @@ private:
 
 public:
   CANPacker(const std::string& dbc_name);
-  uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &signals, int counter);
+  uint64_t pack(uint32_t address, const std::vector<SignalPackValue> &values, int counter);
+  Msg* lookup_message(uint32_t address);
 };

can/common_dbc.h

@@ -3,6 +3,7 @@
 #include <cstddef>
 #include <cstdint>
 #include <string>
+#include <vector>
 
 #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
 
@@ -74,6 +75,7 @@ struct DBC {
   size_t num_vals;
 };
 
+std::vector<const DBC*>& get_dbcs();
 const DBC* dbc_lookup(const std::string& dbc_name);
 
 void dbc_register(const DBC* dbc);

can/dbc.cc

@@ -2,15 +2,11 @@
 
 #include "common_dbc.h"
 
-namespace {
-
 std::vector<const DBC*>& get_dbcs() {
   static std::vector<const DBC*> vec;
   return vec;
 }
 
-}
-
 const DBC* dbc_lookup(const std::string& dbc_name) {
   for (const auto& dbci : get_dbcs()) {
     if (dbc_name == dbci->name) {

can/packer.cc

@@ -112,3 +112,7 @@ uint64_t CANPacker::pack(uint32_t address, const std::vector<SignalPackValue> &s
 
   return ret;
 }
+
+Msg* CANPacker::lookup_message(uint32_t address) {
+  return &message_lookup[address];
+}

can/parser.cc

@@ -13,7 +13,6 @@
 // #define DEBUG printf
 #define INFO printf
 
-
 bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) {
   uint64_t dat_le = read_u64_le(dat);
   uint64_t dat_be = read_u64_be(dat);
@@ -34,47 +33,52 @@ bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) {
 
     DEBUG("parse 0x%X %s -> %lld\n", address, sig.name, tmp);
 
-    if (sig.type == SignalType::HONDA_CHECKSUM) {
+    if (!ignore_checksum) {
-      if (honda_checksum(address, dat_be, size) != tmp) {
+      if (sig.type == SignalType::HONDA_CHECKSUM) {
-        INFO("0x%X CHECKSUM FAIL\n", address);
+        if (honda_checksum(address, dat_be, size) != tmp) {
-        return false;
+          INFO("0x%X CHECKSUM FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
+        if (toyota_checksum(address, dat_be, size) != tmp) {
+          INFO("0x%X CHECKSUM FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
+        if (volkswagen_crc(address, dat_le, size) != tmp) {
+          INFO("0x%X CRC FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::SUBARU_CHECKSUM) {
+        if (subaru_checksum(address, dat_be, size) != tmp) {
+          INFO("0x%X CHECKSUM FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::CHRYSLER_CHECKSUM) {
+        if (chrysler_checksum(address, dat_le, size) != tmp) {
+          INFO("0x%X CHECKSUM FAIL\n", address);
+          return false;
+        }
+      } else if (sig.type == SignalType::PEDAL_CHECKSUM) {
+        if (pedal_checksum(dat_be, size) != tmp) {
+          INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
+          return false;
+        }
       }
-    } else if (sig.type == SignalType::HONDA_COUNTER) {
+    }
-      if (!update_counter_generic(tmp, sig.b2)) {
+    if (!ignore_counter) {
-        return false;
+      if (sig.type == SignalType::HONDA_COUNTER) {
-      }
-    } else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
-      if (toyota_checksum(address, dat_be, size) != tmp) {
-        INFO("0x%X CHECKSUM FAIL\n", address);
-        return false;
-      }
-    } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
-      if (volkswagen_crc(address, dat_le, size) != tmp) {
-        INFO("0x%X CRC FAIL\n", address);
-        return false;
-      }
-    } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
         if (!update_counter_generic(tmp, sig.b2)) {
-        return false;
+          return false;
-      }
+        }
-    } else if (sig.type == SignalType::SUBARU_CHECKSUM) {
+      } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
-      if (subaru_checksum(address, dat_be, size) != tmp) {
+          if (!update_counter_generic(tmp, sig.b2)) {
-        INFO("0x%X CHECKSUM FAIL\n", address);
+          return false;
-        return false;
+        }
-      }
+      } else if (sig.type == SignalType::PEDAL_COUNTER) {
-    } else if (sig.type == SignalType::CHRYSLER_CHECKSUM) {
+        if (!update_counter_generic(tmp, sig.b2)) {
-      if (chrysler_checksum(address, dat_le, size) != tmp) {
+          return false;
-        INFO("0x%X CHECKSUM FAIL\n", address);
+        }
-        return false;
-      }
-    } else if (sig.type == SignalType::PEDAL_CHECKSUM) {
-      if (pedal_checksum(dat_be, size) != tmp) {
-        INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
-        return false;
-      }
-    } else if (sig.type == SignalType::PEDAL_COUNTER) {
-      if (!update_counter_generic(tmp, sig.b2)) {
-        return false;
       }
     }
 
@@ -125,7 +129,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
     }
 
     const Msg* msg = NULL;
-    for (int i=0; i<dbc->num_msgs; i++) {
+    for (int i = 0; i < dbc->num_msgs; i++) {
       if (dbc->msgs[i].address == op.address) {
         msg = &dbc->msgs[i];
         break;
@@ -139,7 +143,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
     state.size = msg->size;
 
     // track checksums and counters for this message
-    for (int i=0; i<msg->num_sigs; i++) {
+    for (int i = 0; i < msg->num_sigs; i++) {
       const Signal *sig = &msg->sigs[i];
       if (sig->type != SignalType::DEFAULT) {
         state.parse_sigs.push_back(*sig);
@@ -151,7 +155,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
     for (const auto& sigop : sigoptions) {
       if (sigop.address != op.address) continue;
 
-      for (int i=0; i<msg->num_sigs; i++) {
+      for (int i = 0; i < msg->num_sigs; i++) {
         const Signal *sig = &msg->sigs[i];
         if (strcmp(sig->name, sigop.name) == 0
             && sig->type == SignalType::DEFAULT) {
@@ -164,45 +168,34 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
   }
 }
 
-void CANParser::UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans) {
+CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter)
-    int msg_count = cans.size();
+  : bus(abus) {
+  // Add all messages and signals
 
-    DEBUG("got %d messages\n", msg_count);
+  dbc = dbc_lookup(dbc_name);
+  assert(dbc);
+  init_crc_lookup_tables();
 
-    // parse the messages
+  for (int i = 0; i < dbc->num_msgs; i++) {
-    for (int i = 0; i < msg_count; i++) {
+    const Msg* msg = &dbc->msgs[i];
-      auto cmsg = cans[i];
+    MessageState state = {
-      if (cmsg.getSrc() != bus) {
+      .address = msg->address,
-        // DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
+      .size = msg->size,
-        continue;
+      .ignore_checksum = ignore_checksum,
-      }
+      .ignore_counter = ignore_counter,
-      auto state_it = message_states.find(cmsg.getAddress());
+    };
-      if (state_it == message_states.end()) {
-        // DEBUG("skip %d: not specified\n", cmsg.getAddress());
-        continue;
-      }
 
-      if (cmsg.getDat().size() > 8) continue; //shouldn't ever happen
+    for (int j = 0; j < msg->num_sigs; j++) {
-      uint8_t dat[8] = {0};
+      const Signal *sig = &msg->sigs[j];
-      memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
+      state.parse_sigs.push_back(*sig);
-
+      state.vals.push_back(0);
-      state_it->second.parse(sec, cmsg.getBusTime(), dat);
     }
-}
 
-void CANParser::UpdateValid(uint64_t sec) {
+    message_states[state.address] = state;
-  can_valid = true;
-  for (const auto& kv : message_states) {
-    const auto& state = kv.second;
-    if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
-      if (state.seen > 0) {
-        DEBUG("0x%X TIMEOUT\n", state.address);
-      }
-      can_valid = false;
-    }
   }
 }
 
+#ifndef DYNAMIC_CAPNP
 void CANParser::update_string(const std::string &data, bool sendcan) {
   // format for board, make copy due to alignment issues.
   const size_t buf_size = (data.length() / sizeof(capnp::word)) + 1;
@@ -223,6 +216,67 @@ void CANParser::update_string(const std::string &data, bool sendcan) {
   UpdateValid(last_sec);
 }
 
+void CANParser::UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans) {
+  int msg_count = cans.size();
+
+  DEBUG("got %d messages\n", msg_count);
+
+  for (int i = 0; i < msg_count; i++) {
+    auto cmsg = cans[i];
+    // parse the messages
+    if (cmsg.getSrc() != bus) {
+      // DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
+      continue;
+    }
+    auto state_it = message_states.find(cmsg.getAddress());
+    if (state_it == message_states.end()) {
+      // DEBUG("skip %d: not specified\n", cmsg.getAddress());
+      continue;
+    }
+
+    if (cmsg.getDat().size() > 8) continue; //shouldn't ever happen
+    uint8_t dat[8] = {0};
+    memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
+
+    state_it->second.parse(sec, cmsg.getBusTime(), dat);
+  }
+}
+#endif
+
+void CANParser::UpdateCans(uint64_t sec, const capnp::DynamicStruct::Reader& cmsg) {
+  // assume message struct is `cereal::CanData` and parse
+  assert(cmsg.has("address") && cmsg.has("src") && cmsg.has("dat") && cmsg.has("busTime"));
+
+  if (cmsg.get("src").as<uint8_t>() != bus) {
+    DEBUG("skip %d: wrong bus\n", cmsg.get("address").as<uint32_t>());
+    return;
+  }
+
+  auto state_it = message_states.find(cmsg.get("address").as<uint32_t>());
+  if (state_it == message_states.end()) {
+    DEBUG("skip %d: not specified\n", cmsg.get("address").as<uint32_t>());
+    return;
+  }
+
+  auto dat = cmsg.get("dat").as<capnp::Data>();
+  if (dat.size() > 8) return; //shouldn't ever happen
+  uint8_t data[8] = {0};
+  memcpy(data, dat.begin(), dat.size());
+  state_it->second.parse(sec, cmsg.get("busTime").as<uint16_t>(), data);
+}
+
+void CANParser::UpdateValid(uint64_t sec) {
+  can_valid = true;
+  for (const auto& kv : message_states) {
+    const auto& state = kv.second;
+    if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
+      if (state.seen > 0) {
+        DEBUG("0x%X TIMEOUT\n", state.address);
+      }
+      can_valid = false;
+    }
+  }
+}
 
 std::vector<SignalValue> CANParser::query_latest() {
   std::vector<SignalValue> ret;

can/process_dbc.py

@@ -106,8 +106,12 @@ def process(in_fn, out_fn):
 
   parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
 
-  with open(out_fn, "w") as out_f:
+  with open(out_fn, "a+") as out_f:
-    out_f.write(parser_code)
+    out_f.seek(0)
+    if out_f.read() != parser_code:
+      out_f.seek(0)
+      out_f.truncate()
+      out_f.write(parser_code)
 
 def main():
   if len(sys.argv) != 3: