Offset time YAML parameter

Author: AHuguetUPC

config/ouster.yaml

@@ -26,6 +26,7 @@ real_time_delay: 0.1       # Should be at least [FULL_ROTATION_TIME] (without a
 
 # LiDAR
 LiDAR_type: ouster       # Options: velodyne, hesai, ouster, custom
+offset_begin: true       # (Usual values: Velodyne = false, Ouster = true, HESAI = indiferent) Is the offset with respect the beginning of the rotation (i.e. point.time ~ [0, 0.1]) or with respect the end (i.e. point.time ~ [-0.1, 0])? For more information see Issue #14: https://github.com/Huguet57/LIMO-Velo/issues/14
 LiDAR_noise: 0.001
 full_rotation_time: 0.1
 min_dist: 4                # Minimum distance: doesn't use points closer than this radius

config/params.yaml

@@ -24,6 +24,7 @@ real_time_delay: 0.1       # Should be at least [FULL_ROTATION_TIME] (without a
 
 # LiDAR
 LiDAR_type: velodyne       # Options: velodyne, hesai, ouster, custom
+offset_begin: false        # (Usual values: Velodyne = false, Ouster = true, HESAI = indiferent) Is the offset with respect the beginning of the rotation (i.e. point.time ~ [0, 0.1]) or with respect the end (i.e. point.time ~ [-0.1, 0])? For more information see Issue #14: https://github.com/Huguet57/LIMO-Velo/issues/14
 LiDAR_noise: 0.001
 full_rotation_time: 0.1
 min_dist: 4                # Minimum distance: doesn't use points closer than this radius

config/xaloc.yaml

@@ -23,6 +23,7 @@ real_time_delay: 0.1       # Should be at least [FULL_ROTATION_TIME] (without a
 
 # LiDAR
 LiDAR_type: velodyne
+offset_begin: true        # (Xaloc's value, having a modified LiDAR driver, is 'true') Is the offset with respect the beginning of the rotation (i.e. point.time ~ [0, 0.1]) or with respect the end (i.e. point.time ~ [-0.1, 0])? For more information see Issue #14: https://github.com/Huguet57/LIMO-Velo/issues/14
 LiDAR_noise: 0.001
 full_rotation_time: 0.1
 min_dist: 4

include/Headers/Accumulator.hpp

@@ -52,6 +52,7 @@ class Accumulator {
 
     private:
         bool is_ready = false;
+        bool has_warned_lidar = false;
 
         void push(const State&);
         void push(const IMU&);
@@ -110,6 +111,9 @@ class Accumulator {
         void set_initial_time();
         double interpret_heuristic(const HeuristicParams&, double t);
 
+        bool missing_data(const Points&);
+        void throw_warning(const Points&);
+
     // Singleton pattern
     public:
         static Accumulator& getInstance() {

include/Headers/Common.hpp

@@ -73,6 +73,8 @@ struct Params {
     double min_dist;
     std::string LiDAR_type;
 
+    bool offset_begin;
+
     double degeneracy_threshold;
     bool print_degeneracy_values;
 

include/Headers/Objects.hpp

@@ -93,14 +93,22 @@ class Point {
             Point(const velodyne_ros::Point& p) {
                 this->set_XYZ(p);
                 this->set_attributes(p);
-                this->time = (double) p.time;
+                
+                // Time offset with respect to beginning of rotation, i.e. ~= [0, 0.1]
+                if (Config.offset_begin) this->time = (double) p.time;
+                // Time offset with respect to end of rotation, i.e. ~= [-0.1, 0]
+                else this->time = Config.full_rotation_time + (double) p.time;
             }
 
         // Ouster specific
             Point(const ouster_ros::Point& p) {
                 this->set_XYZ(p);
                 this->set_attributes(p);
-                this->time = Conversions::nanosec2Sec(p.t);
+
+                // Time offset with respect to beginning of rotation, i.e. ~= [0, 0.1]
+                if (Config.offset_begin) this->time = Conversions::nanosec2Sec(p.t);
+                // Time offset with respect to end of rotation, i.e. ~= [-0.1, 0]
+                else this->time = Config.full_rotation_time + Conversions::nanosec2Sec(p.t);
             }
 
         // Custom specific

src/Modules/Accumulator.cpp

@@ -39,6 +39,10 @@ extern struct Params Config;
             void Accumulator::receive_lidar(const PointCloud_msg& msg) {
                 // Turn message to processed points
                 Points points = this->process(msg);
+                
+                // Check if missing data
+                if (this->missing_data(points)) this->throw_warning(points);
+
                 // Add them individually on the LiDAR buffer
                 for (Point p : points) this->add(p);
             }
@@ -161,4 +165,29 @@ extern struct Params Config;
                     return heuristic.deltas[k];
 
             return heuristic.deltas.back();
+        }
+
+        bool Accumulator::missing_data(const Points& time_sorted_points) {
+            if (time_sorted_points.size() < Config.MAX_POINTS2MATCH) return false;
+            
+            // Check missing 'time' information
+            if (time_sorted_points.front().time == 0 and time_sorted_points.back().time == 0) {
+                // Remove Heuristic
+                Config.Heuristic.times = {};
+                Config.Heuristic.deltas = {Config.full_rotation_time};
+                
+                return true;
+            }
+
+            return false;
+        }
+
+        void Accumulator::throw_warning(const Points& time_sorted_points) {
+            // Warn once
+            if (not this->has_warned_lidar) this->has_warned_lidar = true;
+            else return;
+
+            // Warn missing 'time' information
+            ROS_ERROR("LiDAR points are missing 'time' information.");
+            ROS_ERROR("Delta has been fixed to %f (s) leading to a fixed %d (Hz) localization.", Config.full_rotation_time, (int) 1./Config.full_rotation_time);
         }

src/main.cpp

@@ -163,8 +163,9 @@ void fill_config(ros::NodeHandle& nh) {
     nh.param<double>("wz_MULTIPLIER", Config.wz_MULTIPLIER, 1);
     nh.param<std::string>("points_topic", Config.points_topic, "/velodyne_points");
     nh.param<std::string>("imus_topic", Config.imus_topic, "/vectornav/IMU");
+    nh.param<bool>("offset_begin", Config.offset_begin, false);
     nh.param<std::vector<double>>("/Heuristic/times", Config.Heuristic.times, {});
-    nh.param<std::vector<double>>("/Heuristic/deltas", Config.Heuristic.deltas, {0.1});
+    nh.param<std::vector<double>>("/Heuristic/deltas", Config.Heuristic.deltas, {Config.full_rotation_time});
     nh.param<std::vector<float>>("initial_gravity", Config.initial_gravity, {0.0, 0.0, -9.807});
     nh.param<std::vector<float>>("I_Translation_L", Config.I_Translation_L, std::vector<float> (3, 0.));
     nh.param<std::vector<float>>("I_Rotation_L", Config.I_Rotation_L, std::vector<float> (9, 0.));