Feature/lidar lite

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "dependencies/versavis_hw"]
 	path = dependencies/versavis_hw
 	url = git@github.com:ethz-asl/versavis_hw.git
+[submodule "firmware/libraries/LIDAREnhanced"]
+	path = firmware/libraries/LIDAREnhanced
+	url = git@github.com:rikba/LIDAREnhanced.git

CHANGELOG.rst

@@ -1,7 +1,10 @@
+1.1.0 (TBD)
+------------------
+* Add Lidar Lite sensor
+
 1.0.0 (2019-12-09)
 ------------------
 
 First public *stable* release
 
 Accompanies our paper submission Tschopp, Florian, et al. "VersaVIS: An Open Versatile Multi-Camera Visual-Inertial Sensor Suite." arXiv preprint arXiv:1912.02469 (2019).
-

firmware/libraries/versavis/src/LidarLite.cpp

@@ -0,0 +1,82 @@
+#include "LidarLite.h"
+
+#include "helper.h"
+#include "versavis_configuration.h"
+
+LidarLite::LidarLite(ros::NodeHandle *nh, const String &topic,
+                     const int rate_hz, Timer &timer)
+    : Sensor(nh, topic, rate_hz, timer, range_msg_) {}
+
+void LidarLite::setup() {
+  // The LidarEnhanced driver will configure this GPIO to start/stop the
+  // LidarLite. We map it on an unused pins.
+  const int kLidarEnablePin = 19; // Not connected.
+  lidar_.begin(kLidarEnablePin);
+  const bool kFastI2C = true;
+  lidar_controller_.begin(kFastI2C);
+  delay(100);
+  lidar_controller_.add(&lidar_, kId);
+  setupPublisher();
+}
+
+void LidarLite::begin() {
+  DEBUG_PRINTLN((topic_ + " (LidarLite.cpp): Begin.").c_str());
+  Sensor::setupTimer();
+}
+
+void LidarLite::triggerMeasurement() {
+  // Check whether an overflow caused the interrupt.
+  if (!timer_.checkOverflow()) {
+    DEBUG_PRINTLN(
+        (topic_ + " (LidarLite.cpp): Timer interrupt but not overflown.")
+            .c_str());
+    return;
+  }
+
+  DEBUG_PRINTLN((topic_ + " (LidarLite.cpp): Trigger.").c_str());
+  Sensor::newMeasurementIsAvailable();
+  // Reset the timer.
+  timer_.resetOverflow();
+}
+
+void LidarLite::publish() {
+  if (Sensor::isNewMeasurementAvailable()) {
+    // Get the most recent time stamp.
+    range_msg_.time.data = Sensor::getTimestamp();
+
+    // Get signal strength.
+    uint8_t nack =
+        lidar_controller_.signalStrength(kId, &range_msg_.signal_strength);
+    if (nack) {
+      Sensor::newMeasurementIsNotAvailable();
+      return;
+    }
+
+    // Get range and trigger new measurement.
+    nack = lidar_controller_.distanceAndAsync(kId, &range_msg_.range);
+    // New measurement triggered. Set the next time stamp.
+    Sensor::setTimestampNow();
+    if (nack) {
+      Sensor::newMeasurementIsNotAvailable();
+      return;
+    }
+
+    if (range_msg_.time.data.sec != 0 && range_msg_.time.data.nsec != 0) {
+#ifndef DEBUG
+      publisher_.publish(&range_msg_);
+#endif
+    }
+
+    Sensor::newMeasurementIsNotAvailable();
+  }
+}
+
+void LidarLite::setupPublisher() {
+  publisher_ = ros::Publisher(topic_.c_str(), &range_msg_);
+  DEBUG_PRINT(
+      (topic_ + " (LidarLite.cpp): Setup publisher with topic ").c_str());
+  DEBUG_PRINTLN(publisher_.topic_);
+#ifndef DEBUG
+  nh_->advertise(publisher_);
+#endif
+}

firmware/libraries/versavis/src/LidarLite.h

@@ -0,0 +1,44 @@
+////////////////////////////////////////////////////////////////////////////////
+//  December 2019
+//  Author: Rik Bähnemann <brik@ethz.ch>
+////////////////////////////////////////////////////////////////////////////////
+//  LidarLite.h
+////////////////////////////////////////////////////////////////////////////////
+//
+//  Implementation for Garmin Lidar Lite in the versavis framework. Refer to the
+//  parent package versavis for license information.
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef LidarLite_h
+#define LidarLite_h
+
+#include <LidarController.h>
+#include <LidarObject.h>
+#include <Sensor.h>
+#include <versavis/RangeMicro.h>
+
+class LidarLite : public Sensor {
+public:
+  LidarLite(ros::NodeHandle *nh, const String &topic, const int rate_hz,
+            Timer &timer);
+  void setup() override;
+  void begin() override;
+  void triggerMeasurement() override;
+  void publish() override;
+  void setupPublisher() override;
+
+private:
+  // Disable copy / assignment constructors.
+  LidarLite(const LidarLite &) = delete;
+  LidarLite &operator=(const LidarLite &) = delete;
+
+  const uint8_t kId = 0;
+
+  versavis::RangeMicro range_msg_;
+
+  LidarController lidar_controller_;
+  LidarObject lidar_;
+};
+
+#endif

firmware/libraries/versavis/src/Sensor.cpp

@@ -43,6 +43,27 @@ Sensor::Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
   }
 }
 
+// Range message version.
+Sensor::Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
+               Timer &timer, versavis::RangeMicro &range_msg,
+               const trigger_type type /* = trigger_type::NON_INVERTED */)
+    : nh_(nh), topic_(topic), publisher_(topic.c_str(), &range_msg),
+      new_measurement_available_(false), rate_hz_(rate_hz), timer_(timer),
+      type_(type), max_compare_(pow(2, 16)) {
+  if (nh == nullptr) {
+    error((topic_ + " (Sensor.cpp): The node handle is not available.").c_str(),
+          49);
+  }
+  if (rate_hz <= 0) {
+    error((topic_ + " (Sensor.cpp): The rate of a sensor needs to be positive.")
+              .c_str(),
+          50);
+  }
+  if (topic.length() == 0) {
+    error((topic_ + " (Sensor.cpp): Range topic is empty.").c_str(), 51);
+  }
+}
+
 void Sensor::setTimestampNow() { timestamp_ = nh_->now(); }
 
 ros::Time Sensor::getTimestamp() const { return timestamp_; }

firmware/libraries/versavis/src/Sensor.h

@@ -17,6 +17,7 @@
 #include "Timer.h"
 #include <ros.h>
 #include <versavis/ImuMicro.h>
+#include <versavis/RangeMicro.h>
 #include <versavis/TimeNumbered.h>
 
 enum trigger_type { INVERTED, NON_INVERTED };
@@ -29,6 +30,9 @@ class Sensor {
   Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
          Timer &timer, versavis::ImuMicro &imu_msg,
          const trigger_type type = trigger_type::NON_INVERTED);
+  Sensor(ros::NodeHandle *nh, const String &topic, const int rate_hz,
+         Timer &timer, versavis::RangeMicro &range_msg,
+         const trigger_type type = trigger_type::NON_INVERTED);
   inline virtual void setup(){/* do nothing */};
   inline virtual void begin(){/* do nothing */};
   inline virtual void triggerMeasurement() = 0;

firmware/libraries/versavis/src/versavis_configuration.h

@@ -55,6 +55,14 @@
 #define ILLUMINATION_PIN 26
 #endif
 
+/* ----- Garmin LidarLite ----- */
+// Activation of a Lidar Lite on I2C.
+// #define USE_LIDAR_LITE
+#ifdef USE_LIDAR_LITE
+#define LIDAR_LITE_TOPIC "/versavis/lidar_lite_micro"
+#define LIDAR_LITE_RATE 20
+#endif
+
 /* ----- Debug prints. ----- */
 // Define whether debug mode should be used. This provides output on the
 // standard console but invalidates ROS communication.

firmware/versavis/versavis.ino

@@ -17,6 +17,9 @@
 #include <ADIS16460.h>
 #endif
 #include <Camera.h>
+#ifdef USE_LIDAR_LITE
+#include <LidarLite.h>
+#endif
 #include <Timer.h>
 #include <helper.h>
 
@@ -43,6 +46,9 @@ ros::Subscriber<std_msgs::UInt8> pwm_sub("/versavis/illumination_pwm", &pwmCb);
 Timer timer_cam0 = Timer((Tcc *)TCC0);
 Timer timer_cam1 = Timer((Tcc *)TCC1);
 Timer timer_cam2 = Timer((TcCount16 *)TC3);
+#ifdef USE_LIDAR_LITE
+Timer timer_lidar_lite = Timer((TcCount16 *)TC4);
+#endif
 Timer timer_imu = Timer((TcCount16 *)TC5);
 
 /* ----- IMU ----- */
@@ -64,6 +70,11 @@ Camera cam1(&nh, CAM1_TOPIC, CAM1_RATE, timer_cam1, CAM1_TYPE, CAM1_TRIGGER_PIN,
 Camera cam2(&nh, CAM2_TOPIC, CAM2_RATE, timer_cam2, CAM2_TYPE, CAM2_TRIGGER_PIN,
             CAM2_EXPOSURE_PIN, true);
 
+/* ----- Lidar Lite ----- */
+#ifdef USE_LIDAR_LITE
+LidarLite lidar_lite(&nh, LIDAR_LITE_TOPIC, LIDAR_LITE_RATE, timer_lidar_lite);
+#endif
+
 void setup() {
   DEBUG_INIT(115200);
 
@@ -102,6 +113,9 @@ void setup() {
   cam0.setup();
   cam1.setup();
   cam2.setup();
+#ifdef USE_LIDAR_LITE
+  lidar_lite.setup();
+#endif
 
   /* ----- Initialize all connected cameras. ----- */
   while (!cam0.isInitialized() || !cam1.isInitialized() ||
@@ -131,7 +145,7 @@ void setup() {
     ; // wait for sync
   }
 
-  // Enable TC4 (not used) and TC5 timers.
+  // Enable TC4 (LidarLite) and TC5 timers.
   REG_GCLK_CLKCTRL = static_cast<uint16_t>(
       GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID_TC4_TC5);
   while (GCLK->STATUS.bit.SYNCBUSY == 1) {
@@ -142,12 +156,18 @@ void setup() {
   NVIC_EnableIRQ(TCC0_IRQn);
   NVIC_EnableIRQ(TCC1_IRQn);
   NVIC_EnableIRQ(TC3_IRQn);
+#ifdef USE_LIDAR_LITE
+  NVIC_EnableIRQ(TC4_IRQn);
+#endif
   NVIC_EnableIRQ(TC5_IRQn);
 
   imu.begin();
   cam0.begin();
   cam1.begin();
   cam2.begin();
+#ifdef USE_LIDAR_LITE
+  lidar_lite.begin();
+#endif
 
   /* ----- Interrupt for measuring the exposure time. ----- */
   noInterrupts(); // Disable interrupts to configure them --> delay()'s
@@ -170,6 +190,9 @@ void loop() {
   cam1.publish();
   cam2.publish();
   imu.publish();
+#ifdef USE_LIDAR_LITE
+  lidar_lite.publish();
+#endif
 
 #ifndef DEBUG
   nh.spinOnce();
@@ -188,8 +211,14 @@ void TC3_Handler() { // Called by cam2_timer for camera 2 trigger.
   cam2.triggerMeasurement();
 }
 
-void TC5_Handler() { // Called by imu_timer for imu trigger.
-  imu.triggerMeasurement();
+#ifdef USE_LIDAR_LITE
+void TC4_Handler() { // Called by lidar_lite_timer for lidar lite trigger.
+  lidar_lite.triggerMeasurement();
+}
+#endif
+
+void TC5_Handler() {
+  imu.triggerMeasurement(); // Called by imu_timer for imu trigger.
 }
 
 void exposureEnd0() {

versavis/CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
   FILES
   ImuMicro.msg
+  RangeMicro.msg
   TimeNumbered.msg
 )
 
@@ -68,6 +69,12 @@ add_executable(versavis_imu_receiver
 add_dependencies(versavis_imu_receiver ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(versavis_imu_receiver ${catkin_LIBRARIES})
 
+add_executable(versavis_range_receiver
+   src/versavis_range_receiver.cpp
+)
+add_dependencies(versavis_range_receiver ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(versavis_range_receiver ${catkin_LIBRARIES})
+
 ############
 ## EXPORT ##
 ############

versavis/launch/run_lidar_lite.launch

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+
+<launch>
+  	<!-- Run arduino link. -->
+  <node name="rosserial_python" pkg="rosserial_python" type="serial_node.py"
+    args="_port:=/dev/ttyACM0 _baud:=250000" required="true" output="screen" />
+
+  <!-- Receiver Lidar Lite message. -->
+  <node name="versavis_lidar_lite_receiver" pkg="versavis"
+      type="versavis_range_receiver" required="true" output="screen">
+    <param name="range_sub_topic"           value="/versavis/lidar_lite_micro" />
+    <param name="range_pub_topic"           value="/versavis/lidar_lite" />
+
+    <!-- Parameters from Garmin Lidar Lite v3 specs. -->
+    <!-- https://buy.garmin.com/en-US/US/p/557294#specs -->
+    <param name="frame_id"           value="lidar_lite" />
+    <param name="radiation_type"     value="1" /> <!-- 1 = INFRARED -->
+    <param name="field_of_view"      value="0.008" />
+    <param name="min_range"          value="0.05" />
+    <param name="max_range"          value="40.0" />
+    <param name="range_sensitivity"  value="0.01" />
+    <param name="min_signal_strength"  value="0" /> <!-- No minimum signal strength -->
+  </node>
+
+  <node pkg="rostopic" type="rostopic" name="initializer_cam0" args="pub -r 10 /versavis/cam0/init std_msgs/Bool {'data: True'}"/>
+
+  <node pkg="rostopic" type="rostopic" name="initializer_cam1" args="pub -r 10 /versavis/cam1/init std_msgs/Bool {'data: True'}"/>
+
+  <node pkg="rostopic" type="rostopic" name="initializer_cam2" args="pub -r 10 /versavis/cam2/init std_msgs/Bool {'data: True'}"/>
+</launch>

versavis/msg/RangeMicro.msg

@@ -0,0 +1,11 @@
+# This is a super-minimized message to hold data from a range sensor, such as Garmin LidarLite.
+#
+# Ranges and signal strengths are raw data and have to be scaled!
+#
+# To minimize the amount of data transfered by this message (e.g. form an Arduino through rosserial)
+# the amount of data is truncated to its absolute minimum.
+
+std_msgs/Time time
+
+int16 range
+uint8 signal_strength