From f7aaa032894403f028d8f21e1a69840ad7670215 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Wed, 11 Dec 2019 13:57:48 +0100 Subject: [PATCH 1/2] Port LidarLite extension from private repo. --- .gitmodules | 3 + firmware/libraries/LIDAREnhanced | 1 + firmware/libraries/versavis/src/LidarLite.cpp | 82 +++++++++++ firmware/libraries/versavis/src/LidarLite.h | 44 ++++++ firmware/libraries/versavis/src/Sensor.cpp | 21 +++ firmware/libraries/versavis/src/Sensor.h | 4 + .../versavis/src/versavis_configuration.h | 8 ++ firmware/versavis/versavis.ino | 35 ++++- versavis/CMakeLists.txt | 7 + versavis/launch/run_lidar_lite.launch | 30 ++++ versavis/msg/RangeMicro.msg | 11 ++ versavis/src/versavis_range_receiver.cpp | 135 ++++++++++++++++++ 12 files changed, 378 insertions(+), 3 deletions(-) create mode 160000 firmware/libraries/LIDAREnhanced create mode 100644 firmware/libraries/versavis/src/LidarLite.cpp create mode 100644 firmware/libraries/versavis/src/LidarLite.h create mode 100644 versavis/launch/run_lidar_lite.launch create mode 100644 versavis/msg/RangeMicro.msg create mode 100644 versavis/src/versavis_range_receiver.cpp diff --git a/.gitmodules b/.gitmodules index 9a02d802..81b953f9 100644 --- a/.gitmodules +++ b/.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 diff --git a/firmware/libraries/LIDAREnhanced b/firmware/libraries/LIDAREnhanced new file mode 160000 index 00000000..125bbfbe --- /dev/null +++ b/firmware/libraries/LIDAREnhanced @@ -0,0 +1 @@ +Subproject commit 125bbfbe60d0c063377390b66733a062776a681d diff --git a/firmware/libraries/versavis/src/LidarLite.cpp b/firmware/libraries/versavis/src/LidarLite.cpp new file mode 100644 index 00000000..8ae8491c --- /dev/null +++ b/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 +} diff --git a/firmware/libraries/versavis/src/LidarLite.h b/firmware/libraries/versavis/src/LidarLite.h new file mode 100644 index 00000000..40b74fb9 --- /dev/null +++ b/firmware/libraries/versavis/src/LidarLite.h @@ -0,0 +1,44 @@ +//////////////////////////////////////////////////////////////////////////////// +// December 2019 +// Author: Rik Bähnemann +//////////////////////////////////////////////////////////////////////////////// +// 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 +#include +#include +#include + +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 diff --git a/firmware/libraries/versavis/src/Sensor.cpp b/firmware/libraries/versavis/src/Sensor.cpp index 40d82aa6..e9210321 100644 --- a/firmware/libraries/versavis/src/Sensor.cpp +++ b/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_; } diff --git a/firmware/libraries/versavis/src/Sensor.h b/firmware/libraries/versavis/src/Sensor.h index a92ae330..0a7d5943 100644 --- a/firmware/libraries/versavis/src/Sensor.h +++ b/firmware/libraries/versavis/src/Sensor.h @@ -17,6 +17,7 @@ #include "Timer.h" #include #include +#include #include 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; diff --git a/firmware/libraries/versavis/src/versavis_configuration.h b/firmware/libraries/versavis/src/versavis_configuration.h index 83ef8835..a7f15e0e 100644 --- a/firmware/libraries/versavis/src/versavis_configuration.h +++ b/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. diff --git a/firmware/versavis/versavis.ino b/firmware/versavis/versavis.ino index fe20a9c9..c7cd8e17 100644 --- a/firmware/versavis/versavis.ino +++ b/firmware/versavis/versavis.ino @@ -17,6 +17,9 @@ #include #endif #include +#ifdef USE_LIDAR_LITE +#include +#endif #include #include @@ -43,6 +46,9 @@ ros::Subscriber 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( 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() { diff --git a/versavis/CMakeLists.txt b/versavis/CMakeLists.txt index 49c9772f..053bfffe 100644 --- a/versavis/CMakeLists.txt +++ b/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 ## ############ diff --git a/versavis/launch/run_lidar_lite.launch b/versavis/launch/run_lidar_lite.launch new file mode 100644 index 00000000..088236d0 --- /dev/null +++ b/versavis/launch/run_lidar_lite.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/versavis/msg/RangeMicro.msg b/versavis/msg/RangeMicro.msg new file mode 100644 index 00000000..f3803752 --- /dev/null +++ b/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 diff --git a/versavis/src/versavis_range_receiver.cpp b/versavis/src/versavis_range_receiver.cpp new file mode 100644 index 00000000..c6555fb0 --- /dev/null +++ b/versavis/src/versavis_range_receiver.cpp @@ -0,0 +1,135 @@ +//////////////////////////////////////////////////////////////////////////////// +// December 2019 +// Author: Rik Bähnemann +//////////////////////////////////////////////////////////////////////////////// +// versavis_range_reciever.cpp +//////////////////////////////////////////////////////////////////////////////// +// +// Used to convert versavis/RangeMicro to sensor_msgs/Range. Performs scaling, +// signal strength, and range filtering. +// +//////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include +#include +#include + +#include +#include + +class VersaVisRangeReceiver { +public: + VersaVisRangeReceiver(ros::NodeHandle &node_handle) + : node_handle_(node_handle) { + ROS_INFO("Versavis range message receiver. Version 1.0"); + readParameters(); + range_sub_ = node_handle_.subscribe( + range_sub_topic_, 100u, &VersaVisRangeReceiver::rangeCallback, this); + range_pub_ = + node_handle_.advertise(range_pub_topic_, 100u); + } + + ~VersaVisRangeReceiver() { node_handle_.shutdown(); } + + void rangeCallback(const versavis::RangeMicro &range_micro_msg) { + ROS_INFO_ONCE("Received first Range message from topic: %s", + range_sub_topic_.c_str()); + if (range_micro_msg.time.data.toNSec() < last_msg_time_.toNSec()) { + ROS_WARN("Range message from topic %s is not strictly increasing (%ld vs " + "%ld). Dropping this message. This is normal during startup (< " + "1 min).", + range_sub_topic_.c_str(), range_micro_msg.time.data.toNSec(), + last_msg_time_.toNSec()); + return; + } + last_msg_time_ = range_micro_msg.time.data; + + // Populate range measurement. + range_msg_.header.stamp = range_micro_msg.time.data; + range_msg_.range = rangeScale(range_micro_msg.range); + + // Filter messages out of bounds. + if (range_msg_.range < range_msg_.min_range || + range_msg_.range > range_msg_.max_range) { + ROS_DEBUG( + "Discarding range message out of bounds RANGE: %f MIN: %f MAX: %f.", + range_msg_.range, range_msg_.min_range, range_msg_.max_range); + return; + } + + // Filter messages with bad signal strength. + if (range_micro_msg.signal_strength < min_signal_strength_) { + ROS_DEBUG("Discarding range message bad signal strength: %d MIN: %d.", + range_micro_msg.signal_strength, min_signal_strength_); + return; + } + + range_pub_.publish(range_msg_); + } + + void readParameters() { + ROS_INFO("Read ROS parameters."); + node_handle_.param("range_sub_topic", range_sub_topic_, + std::string("/versavis/range_micro")); + node_handle_.param("range_pub_topic", range_pub_topic_, + std::string("/versavis/range")); + + std::string frame_id = "range_sensor"; + node_handle_.param("frame_id", frame_id, frame_id); + range_msg_.header.frame_id = frame_id; + + int radiation_type = sensor_msgs::Range::INFRARED; + node_handle_.param("radiation_type", radiation_type, radiation_type); + range_msg_.radiation_type = static_cast(radiation_type); + + node_handle_.param("field_of_view", range_msg_.field_of_view, + static_cast(2.0 * M_PI)); + node_handle_.param("min_range", range_msg_.min_range, + std::numeric_limits::lowest()); + node_handle_.param("max_range", range_msg_.max_range, + std::numeric_limits::max()); + node_handle_.param("range_sensitivity", range_sensitivity_, + range_sensitivity_); + + int min_signal_strength = min_signal_strength_; + node_handle_.param("min_signal_strength", min_signal_strength, + min_signal_strength); + // Clamp signal strength value. + min_signal_strength_ = std::max(0, min_signal_strength); + min_signal_strength_ = + std::min(min_signal_strength_, std::numeric_limits::max()); + } + +private: + ros::NodeHandle node_handle_; + ros::Subscriber range_sub_; + ros::Publisher range_pub_; + std::string range_sub_topic_; + std::string range_pub_topic_; + ros::Time last_msg_time_; + + sensor_msgs::Range range_msg_; + double range_sensitivity_ = 1.0; + + uint8_t min_signal_strength_ = 0; + + // Converts raw range data to distance in meters. + inline float rangeScale(const int16_t sensor_data) const { + return static_cast(sensor_data * range_sensitivity_); + } +}; + +int main(int argc, char **argv) { + ros::init(argc, argv, "versavis_range_receiver"); + ros::NodeHandle node_handle("~"); + VersaVisRangeReceiver receiver(node_handle); + + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::waitForShutdown(); + + return 0; +} From 7f1d6187a57bb7c72abe382a708ff0c93f26e708 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rik=20B=C3=A4hnemann?= Date: Wed, 11 Dec 2019 14:01:22 +0100 Subject: [PATCH 2/2] Update changelog. --- CHANGELOG.rst | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 48c8eea2..ee88565d 100644 --- a/CHANGELOG.rst +++ b/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). -