Feat integrate realsense

laserscan_kinect/cfg/LaserscanKinect.cfg

@@ -16,5 +16,8 @@ gen.add("sensor_tilt_angle",    double_t,  0,  "Sensor tilt angle (in degrees)."
 gen.add("ground_remove_en",     bool_t,    0,  "Remove ground from scan.",                   False)
 gen.add("ground_margin",        double_t,  0,  "Ground margin (m).",                         0.05, 0.0, 0.5)
 gen.add("tilt_compensation_en", bool_t,    0,  "Sensor tilt angle compensation.",            False)
+gen.add("slope_detection", 		bool_t,    0,  "Detect slope and remove from scan.",         False)
+gen.add("min_diff", 			double_t,  0,  "Max difference in depths.",            	     0.007)
+
 
 exit(gen.generate("laserscan_kinect", "laserscan_kinect", "LaserscanKinect"))

laserscan_kinect/config/params.yaml

@@ -22,3 +22,8 @@ ground_margin: 0.05
 # Sensor tilt angle compensation.
 tilt_compensation_en: true
 
+# Enable slope detection.
+slope_detection: false
+# Min difference in depths between image rows to be considered a slope.
+min_diff: 0.0
+

laserscan_kinect/include/laserscan_kinect/laserscan_kinect.h

@@ -128,6 +128,18 @@ class LaserScanKinect
    * @param enable
    */
     void setScanConfigurated (const bool configurated) { is_scan_msg_configurated_ = configurated; }
+    /**
+   * @brief setSlopeDetection enables or disables the ramp detection feature
+   *
+   * @param enable
+   */
+    void setSlopeDetection (const bool enable) { slope_detection_ = enable; }
+    /**
+   * @brief setMinDiff sets the minimum difference in depths allowed
+   *
+   * @param min_diff
+   */
+    void setMinDiff (const bool min_diff) { min_diff_ = min_diff; }
 
 protected:
     /**
@@ -176,6 +188,11 @@ class LaserScanKinect
         const unsigned range_min_mm = range_min_ * 1000;
         const unsigned range_max_mm = range_max_ * 1000;
 
+        // The ramp detection algorithm works by looping over the rows in the column so if the current
+        // point is n then depth_n_1 is the depth at the n-1 point and depth_n_2 is the depth at the n-2 point
+        float depth_n_1 = -1000;
+        float depth_n_2 = -1000;
+
         // Loop over pixels in column. Calculate z_min in column
         for (size_t i = image_vertical_offset_; i < image_vertical_offset_ + scan_height_;
                                                                                 i += depth_img_row_step_)
@@ -206,17 +223,37 @@ class LaserScanKinect
                 {
                     if (depth_m < depth_min && depth_raw_mm < dist_to_ground_corrected[i])
                     {
-                        depth_min = depth_m;
+                        if (slope_detection_)
+                        {
+                            // Check the detected object is a slope
+                            if ((depth_n_1 - depth_n_2) > min_diff_ && (depth_m - depth_n_1) > min_diff_)
+                            {
+                              depth_min = depth_m;
+                            }
+                        }
+                        else
+                          depth_min = depth_m;
                     }
                 }
                 else
                 {
                     if (depth_m < depth_min)
                     {
-                        depth_min = depth_m;
+                        if (slope_detection_)
+                        {
+                            // Check the detected object is a slope
+                            if ((depth_n_1 - depth_n_2) > min_diff_ && (depth_m - depth_n_1) > min_diff_)
+                            {
+                              depth_min = depth_m;
+                            }
+                        }
+                        else
+                          depth_min = depth_m;
                     }
                 }
             }
+            depth_n_2 = depth_n_1;
+            depth_n_1 = depth_m;
         }
         return depth_min;
     }
@@ -242,6 +279,8 @@ class LaserScanKinect
     bool  ground_remove_enable_{false};     ///< Determines if remove ground from output scan
     float ground_margin_{0};                ///< Margin for floor remove feature (in meters)
     bool  tilt_compensation_enable_{false}; ///< Determines if tilt compensation feature is on
+    bool  slope_detection_{false};           ///< Determines if a slope should be removed from scan
+    float min_diff_{0};                     ///< Min difference in depths when detecting slopes
     //-----------------------------------------------------------------------------------------------
 
     /// Published scan message

laserscan_kinect/src/laserscan_kinect_node.cpp

@@ -115,6 +115,9 @@ void LaserScanKinectNode::reconfigureCb(laserscan_kinect::LaserscanKinectConfig&
     converter_.setGroundMargin(config.ground_margin);
     converter_.setTiltCompensation(config.tilt_compensation_en);
 
+    converter_.setSlopeDetection(config.slope_detection);
+    converter_.setMinDiff(config.min_diff);
+
     converter_.setScanConfigurated(false);
 }