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/CHANGELOG.rst b/CHANGELOG.rst index c7de1cca..01a5f705 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,3 +1,7 @@ +1.1.0 (TBD) + +* Add Lidar Lite sensor + 1.0.1 (2020-03-12) ------------------ * Added support for Vectornav VN-100 @@ -6,6 +10,7 @@ Details in PR7_ .. _PR7: https://github.com/ethz-asl/versavis/pull/7 + 1.0.0 (2019-12-09) ------------------ 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 816b84a9..8d1fac8f 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 mode. ----- */ // 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 b1fcc802..f9598023 100644 --- a/firmware/versavis/versavis.ino +++ b/firmware/versavis/versavis.ino @@ -19,6 +19,9 @@ #include #endif #include +#ifdef USE_LIDAR_LITE +#include +#endif #include #include @@ -45,6 +48,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 ----- */ @@ -68,6 +74,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); @@ -106,6 +117,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() || @@ -135,7 +149,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) { @@ -146,12 +160,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 @@ -174,6 +194,9 @@ void loop() { cam1.publish(); cam2.publish(); imu.publish(); +#ifdef USE_LIDAR_LITE + lidar_lite.publish(); +#endif #ifndef DEBUG nh.spinOnce(); @@ -192,8 +215,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; +}