Feat integrate realsense

laserscan_kinect/config/params.yaml

@@ -1,24 +1,26 @@
 # Frame id for the output laserscan.
 output_frame_id: camera_depth_frame
+# Output topic name
+topic_name: realsense_1
 # Minimum sensor range (m).
 range_min: 0.4
 # Maximum sensor range (m).
-range_max: 5.0
+range_max: 10.0
 # Height of used part of depth img (px).
-scan_height: 400
+scan_height: 480
 # Row step in depth image processing (px).
 depth_img_row_step: 1
 # If continously camera data update.
 cam_model_update: false
 
 # Height of sensor optical center mount (m).
-sensor_mount_height: 0.2
+sensor_mount_height: 0.36
 # Sensor tilt angle compensation.
-sensor_tilt_angle: 5.0
+sensor_tilt_angle: 15.0
 # Remove ground from scan.
 ground_remove_en: true
 # Ground margin (m).
-ground_margin: 0.05
+ground_margin: 0.01
 # Sensor tilt angle compensation.
 tilt_compensation_en: true
 

laserscan_kinect/include/laserscan_kinect/laserscan_kinect.h

@@ -175,6 +175,9 @@ class LaserScanKinect
         float depth_min = std::numeric_limits<float>::max();
         const unsigned range_min_mm = range_min_ * 1000;
         const unsigned range_max_mm = range_max_ * 1000;
+        float depth_prev_prev = -1000;
+        float depth_prev = -1000;
+        float delta_1 = 10;
 
         // Loop over pixels in column. Calculate z_min in column
         for (size_t i = image_vertical_offset_; i < image_vertical_offset_ + scan_height_;
@@ -206,17 +209,35 @@ class LaserScanKinect
                 {
                     if (depth_m < depth_min && depth_raw_mm < dist_to_ground_corrected[i])
                     {
-                        depth_min = depth_m;
+                        // Check the detected object is a slope
+                        if ((depth_prev - depth_prev_prev) < 0.007 && (depth_m - depth_prev) < 0.007)
+                        {
+                          if(depth_m - depth_prev_prev > delta_1)
+                          {
+                            depth_min = depth_m;
+                          }
+                        }
+
                     }
                 }
                 else
                 {
                     if (depth_m < depth_min)
                     {
-                        depth_min = depth_m;
+                        // Check the detected object is a slope
+                        if ((depth_prev - depth_prev_prev) < 0.007 && (depth_m - depth_prev) < 0.007)
+                        {
+                          if(depth_m - depth_prev_prev > delta_1)
+                          {
+                            depth_min = depth_m;
+                          }
+                        }
                     }
                 }
             }
+
+            depth_prev_prev = depth_prev;
+            depth_prev = depth_m;
         }
         return depth_min;
     }

laserscan_kinect/src/laserscan_kinect_node.cpp

@@ -79,24 +79,24 @@ void LaserScanKinectNode::connectCb(const ros::SingleSubscriberPublisher& pub)
 {
     std::lock_guard<std::mutex> lock(connect_mutex_);
 
-    if (!sub_ && pub_.getNumSubscribers() > 0)
-    {
+    // if (!sub_ && pub_.getNumSubscribers() > 0)
+    // {
         ROS_DEBUG("Connecting to depth topic.");
         image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
         sub_ = it_.subscribeCamera("image", 10, &LaserScanKinectNode::depthCb, this, hints);
-    }
+    // }
 }
 
 //=================================================================================================
 void LaserScanKinectNode::disconnectCb(const ros::SingleSubscriberPublisher& pub)
 {
-    std::lock_guard<std::mutex> lock(connect_mutex_);
+    // std::lock_guard<std::mutex> lock(connect_mutex_);
 
-    if (pub_.getNumSubscribers() == 0)
-    {
-        ROS_DEBUG("Unsubscribing from depth topic.");
-        sub_.shutdown();
-    }
+    // if (pub_.getNumSubscribers() == 0)
+    // {
+    //     ROS_DEBUG("Unsubscribing from depth topic.");
+    //     sub_.shutdown();
+    // }
 }
 
 //=================================================================================================