cabana: support direct streaming from panda over USB (#27936)

* refactor livestream into devicestream

* add panda stream

* unused

* whitespace

* move logging to base class

* add cmdline args

* Update selfdrive/boardd/boardd.cc

---------

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 2a981f553162ff41dd50ed6921b90ac512efa3d2

selfdrive/boardd/SConscript

@@ -1,7 +1,9 @@
 Import('env', 'envCython', 'common', 'cereal', 'messaging')
 
 libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
-env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'panda_comms.cc', 'spi.cc'], LIBS=libs)
+panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc'])
+
+env.Program('boardd', ['main.cc', 'boardd.cc'], LIBS=[panda] + libs)
 env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
 
 envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])

selfdrive/boardd/panda.cc

@@ -21,9 +21,6 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
 
   hw_type = get_hw_type();
 
-  assert((hw_type != cereal::PandaState::PandaType::WHITE_PANDA) &&
-         (hw_type != cereal::PandaState::PandaType::GREY_PANDA));
-
   has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) ||
             (hw_type == cereal::PandaState::PandaType::DOS) ||
             (hw_type == cereal::PandaState::PandaType::TRES);

tools/cabana/README.md

@@ -11,19 +11,23 @@ $ ./cabana -h
 Usage: ./cabana [options] route
 
 Options:
-  -h, --help             Displays this help.
-  --demo                 use a demo route instead of providing your own
-  --qcam                 load qcamera
-  --ecam                 load wide road camera
-  --stream               read can messages from live streaming
-  --zmq <zmq>            the ip address on which to receive zmq messages
-  --data_dir <data_dir>  local directory with routes
-  --no-vipc              do not output video
-  --dbc <dbc>            dbc file to open
+  -h, --help                     Displays help on commandline options.
+  --help-all                     Displays help including Qt specific options.
+  --demo                         use a demo route instead of providing your own
+  --qcam                         load qcamera
+  --ecam                         load wide road camera
+  --stream                       read can messages from live streaming
+  --panda                        read can messages from panda
+  --panda-serial <panda-serial>  read can messages from panda with given serial
+  --zmq <zmq>                    the ip address on which to receive zmq
+                                 messages
+  --data_dir <data_dir>          local directory with routes
+  --no-vipc                      do not output video
+  --dbc <dbc>                    dbc file to open
 
 Arguments:
-  route                  the drive to replay. find your drives at
-                         connect.comma.ai
+  route                          the drive to replay. find your drives at
+                                 connect.comma.ai
 ```
 
 See [openpilot wiki](https://github.com/commaai/openpilot/wiki/Cabana)

tools/cabana/SConscript

@@ -17,7 +17,7 @@ qt_libs = ['qt_util'] + base_libs
 
 cabana_env = qt_env.Clone()
 cabana_env["LIBPATH"] += ['../../opendbc/can']
-cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'libdbc_static', 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs
+cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'panda', 'libdbc_static', 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv', 'usb-1.0'] + qt_libs
 opendbc_path = '-DOPENDBC_FILE_PATH=\'"%s"\'' % (cabana_env.Dir("../../opendbc").abspath)
 cabana_env['CXXFLAGS'] += [opendbc_path]
 
@@ -29,7 +29,7 @@ cabana_env.Depends(assets, Glob('/assets/*', exclude=[assets, assets_src, "asset
 
 prev_moc_path = cabana_env['QT_MOCHPREFIX']
 cabana_env['QT_MOCHPREFIX'] = os.path.dirname(prev_moc_path) + '/cabana/moc_'
-cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'historylog.cc', 'videowidget.cc', 'signalview.cc', 
+cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/pandastream.cc', 'streams/devicestream.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'historylog.cc', 'videowidget.cc', 'signalview.cc', 
                                                'dbc/dbc.cc', 'dbc/dbcfile.cc', 'dbc/dbcmanager.cc',
                                                'chart/chartswidget.cc', 'chart/chart.cc', 'chart/signalselector.cc', 'chart/tiplabel.cc', 'chart/sparkline.cc',
                                                'commands.cc', 'messageswidget.cc', 'route.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks)

tools/cabana/cabana.cc

@@ -5,7 +5,8 @@
 #include "selfdrive/ui/qt/util.h"
 #include "tools/cabana/mainwin.h"
 #include "tools/cabana/route.h"
-#include "tools/cabana/streams/livestream.h"
+#include "tools/cabana/streams/devicestream.h"
+#include "tools/cabana/streams/pandastream.h"
 #include "tools/cabana/streams/replaystream.h"
 
 int main(int argc, char *argv[]) {
@@ -24,6 +25,8 @@ int main(int argc, char *argv[]) {
   cmd_parser.addOption({"qcam", "load qcamera"});
   cmd_parser.addOption({"ecam", "load wide road camera"});
   cmd_parser.addOption({"stream", "read can messages from live streaming"});
+  cmd_parser.addOption({"panda", "read can messages from panda"});
+  cmd_parser.addOption({"panda-serial", "read can messages from panda with given serial", "panda-serial"});
   cmd_parser.addOption({"zmq", "the ip address on which to receive zmq messages", "zmq"});
   cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"});
   cmd_parser.addOption({"no-vipc", "do not output video"});
@@ -34,7 +37,13 @@ int main(int argc, char *argv[]) {
   std::unique_ptr<AbstractStream> stream;
 
   if (cmd_parser.isSet("stream")) {
-    stream.reset(new LiveStream(&app, cmd_parser.value("zmq")));
+    stream.reset(new DeviceStream(&app, cmd_parser.value("zmq")));
+  } else if (cmd_parser.isSet("panda") || cmd_parser.isSet("panda-serial")) {
+    PandaStreamConfig config = {};
+    if (cmd_parser.isSet("panda-serial")) {
+      config.serial = cmd_parser.value("panda-serial");
+    }
+    stream.reset(new PandaStream(&app, config));
   } else {
     // TODO: Remove when OpenpilotPrefix supports ZMQ
 #ifndef __APPLE__

tools/cabana/streams/devicestream.cc

@@ -0,0 +1,27 @@
+#include "tools/cabana/streams/devicestream.h"
+
+DeviceStream::DeviceStream(QObject *parent, QString address) : zmq_address(address), LiveStream(parent) {
+}
+
+void DeviceStream::streamThread() {
+  if (!zmq_address.isEmpty()) {
+    setenv("ZMQ", "1", 1);
+  }
+
+  std::unique_ptr<Context> context(Context::create());
+  std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString();
+  std::unique_ptr<SubSocket> sock(SubSocket::create(context.get(), "can", address));
+  assert(sock != NULL);
+  sock->setTimeout(50);
+  // run as fast as messages come in
+  while (!QThread::currentThread()->isInterruptionRequested()) {
+    Message *msg = sock->receive(true);
+    if (!msg) {
+      QThread::msleep(50);
+      continue;
+    }
+
+    std::lock_guard lk(lock);
+    handleEvent(messages.emplace_back(msg).event);
+  }
+}

tools/cabana/streams/devicestream.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "tools/cabana/streams/livestream.h"
+
+class DeviceStream : public LiveStream {
+  Q_OBJECT
+public:
+  DeviceStream(QObject *parent, QString address = {});
+
+  inline QString routeName() const override {
+    return QString("Live Streaming From %1").arg(zmq_address.isEmpty() ? "127.0.0.1" : zmq_address);
+  }
+
+protected:
+  void streamThread() override;
+  const QString zmq_address;
+};

tools/cabana/streams/livestream.cc

@@ -2,7 +2,13 @@
 
 #include <QTimer>
 
-LiveStream::LiveStream(QObject *parent, QString address) : zmq_address(address), AbstractStream(parent, true) {
+LiveStream::LiveStream(QObject *parent) : AbstractStream(parent, true) {
+  if (settings.log_livestream) {
+    std::string path = (settings.log_path + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd--hh-mm-ss") + "--0").toStdString();
+    util::create_directories(path, 0755);
+    fs.reset(new std::ofstream(path + "/rlog" , std::ios::binary | std::ios::out));
+  }
+
   stream_thread = new QThread(this);
   QObject::connect(stream_thread, &QThread::started, [=]() { streamThread(); });
   QObject::connect(stream_thread, &QThread::finished, stream_thread, &QThread::deleteLater);
@@ -15,39 +21,12 @@ LiveStream::~LiveStream() {
   stream_thread->wait();
 }
 
-void LiveStream::streamThread() {
-  if (!zmq_address.isEmpty()) {
-    setenv("ZMQ", "1", 1);
-  }
-
-  std::unique_ptr<std::ofstream> fs;
-  if (settings.log_livestream) {
-    std::string path = (settings.log_path + "/" + QDateTime::currentDateTime().toString("yyyy-MM-dd--hh-mm-ss") + "--0").toStdString();
-    util::create_directories(path, 0755);
-    fs.reset(new std::ofstream(path + "/rlog" , std::ios::binary | std::ios::out));
-  }
-  std::unique_ptr<Context> context(Context::create());
-  std::string address = zmq_address.isEmpty() ? "127.0.0.1" : zmq_address.toStdString();
-  std::unique_ptr<SubSocket> sock(SubSocket::create(context.get(), "can", address));
-  assert(sock != NULL);
-  sock->setTimeout(50);
-  // run as fast as messages come in
-  while (!QThread::currentThread()->isInterruptionRequested()) {
-    Message *msg = sock->receive(true);
-    if (!msg) {
-      QThread::msleep(50);
-      continue;
-    }
-
-    if (fs) {
-      fs->write(msg->getData(), msg->getSize());
-    }
-    std::lock_guard lk(lock);
-    handleEvent(messages.emplace_back(msg).event);
-  }
-}
-
 void LiveStream::handleEvent(Event *evt) {
+  if (fs) {
+    auto bytes = evt->words.asChars();
+    fs->write(bytes.begin(), bytes.size());
+  }
+
   if (start_ts == 0 || evt->mono_time < start_ts) {
     if (evt->mono_time < start_ts) {
       qDebug() << "stream is looping back to old time stamp";

tools/cabana/streams/livestream.h

@@ -6,11 +6,8 @@ class LiveStream : public AbstractStream {
   Q_OBJECT
 
 public:
-  LiveStream(QObject *parent, QString address = {});
+  LiveStream(QObject *parent);
   virtual ~LiveStream();
-  inline QString routeName() const override {
-    return QString("Live Streaming From %1").arg(zmq_address.isEmpty() ? "127.0.0.1" : zmq_address);
-  }
   inline double routeStartTime() const override { return start_ts / (double)1e9; }
   inline double currentSec() const override { return (current_ts - start_ts) / (double)1e9; }
   void setSpeed(float speed) override { speed_ = std::min<float>(1.0, speed); }
@@ -19,7 +16,7 @@ public:
 
 protected:
   virtual void handleEvent(Event *evt);
-  virtual void streamThread();
+  virtual void streamThread() = 0;
   void process(QHash<MessageId, CanData> *) override;
 
   struct Msg {
@@ -27,6 +24,9 @@ protected:
       event = ::new Event(aligned_buf.align(m));
       delete m;
     }
+    Msg(const char *data, const size_t size) {
+      event = ::new Event(aligned_buf.align(data, size));
+    }
     ~Msg() { ::delete event; }
     Event *event;
     AlignedBuffer aligned_buf;
@@ -41,6 +41,7 @@ protected:
   std::atomic<bool> pause_ = false;
   uint64_t last_update_ts = 0;
 
-  const QString zmq_address;
+  std::unique_ptr<std::ofstream> fs;
+
   QThread *stream_thread;
 };

tools/cabana/streams/pandastream.cc

@@ -0,0 +1,85 @@
+#include "tools/cabana/streams/pandastream.h"
+
+PandaStream::PandaStream(QObject *parent, PandaStreamConfig config_) : config(config_), LiveStream(parent) {
+  if (config.serial.isEmpty()) {
+    auto serials = Panda::list();
+    if (serials.size() == 0) {
+      throw std::runtime_error("No panda found");
+    }
+    config.serial = QString::fromStdString(serials[0]);
+  }
+
+  qDebug() << "Connecting to panda with serial" << config.serial;
+  if (!connect()) {
+    throw std::runtime_error("Failed to connect to panda");
+  }
+}
+
+bool PandaStream::connect() {
+  try {
+    panda.reset(new Panda(config.serial.toStdString()));
+    config.bus_config.resize(3);
+    qDebug() << "Connected";
+  } catch (const std::exception& e) {
+    return false;
+  }
+
+  panda->set_safety_model(cereal::CarParams::SafetyModel::SILENT);
+
+  for (int bus = 0; bus < config.bus_config.size(); bus++) {
+    panda->set_can_speed_kbps(bus, config.bus_config[bus].can_speed_kbps);
+
+    // CAN-FD
+    if (panda->hw_type == cereal::PandaState::PandaType::RED_PANDA || panda->hw_type == cereal::PandaState::PandaType::RED_PANDA_V2) {
+      if (config.bus_config[bus].can_fd) {
+        panda->set_data_speed_kbps(bus, config.bus_config[bus].data_speed_kbps);
+      } else {
+        // Hack to disable can-fd by setting data speed to a low value
+        panda->set_data_speed_kbps(bus, 10);
+      }
+    }
+
+  }
+  return true;
+}
+
+void PandaStream::streamThread() {
+  std::vector<can_frame> raw_can_data;
+
+  while (!QThread::currentThread()->isInterruptionRequested()) {
+    QThread::msleep(1);
+
+    if (!panda->connected()) {
+      qDebug() << "Connection to panda lost. Attempting reconnect.";
+      if (!connect()){
+        QThread::msleep(1000);
+        continue;
+      }
+    }
+
+    raw_can_data.clear();
+    if (!panda->can_receive(raw_can_data)) {
+      qDebug() << "failed to receive";
+      continue;
+    }
+
+    MessageBuilder msg;
+    auto evt = msg.initEvent();
+    auto canData = evt.initCan(raw_can_data.size());
+
+    for (uint i = 0; i<raw_can_data.size(); i++) {
+      canData[i].setAddress(raw_can_data[i].address);
+      canData[i].setBusTime(raw_can_data[i].busTime);
+      canData[i].setDat(kj::arrayPtr((uint8_t*)raw_can_data[i].dat.data(), raw_can_data[i].dat.size()));
+      canData[i].setSrc(raw_can_data[i].src);
+    }
+
+    {
+      std::lock_guard lk(lock);
+      auto bytes = msg.toBytes();
+      handleEvent(messages.emplace_back((const char*)bytes.begin(), bytes.size()).event);
+    }
+
+    panda->send_heartbeat(false);
+  }
+}

tools/cabana/streams/pandastream.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "tools/cabana/streams/livestream.h"
+#include "selfdrive/boardd/panda.h"
+
+struct BusConfig {
+  int can_speed_kbps = 500;
+  int data_speed_kbps = 2000;
+  bool can_fd = false;
+};
+
+struct PandaStreamConfig {
+  QString serial = "";
+  std::vector<BusConfig> bus_config;
+};
+
+class PandaStream : public LiveStream {
+  Q_OBJECT
+public:
+  PandaStream(QObject *parent, PandaStreamConfig config_ = {});
+
+  inline QString routeName() const override {
+    return QString("Live Streaming From Panda %1").arg(config.serial);
+  }
+
+protected:
+  void streamThread() override;
+  bool connect();
+
+  std::unique_ptr<Panda> panda;
+  PandaStreamConfig config = {};
+};
+