scan_offset parameter

cfg/Depth.cfg

@@ -9,6 +9,7 @@ from math import pi
 gen = ParameterGenerator()
 #       Name                    Type      Reconfiguration level             Description                                                            Default    Min   Max
 gen.add("scan_height",          int_t,    0,                                "Height of the laser band (in pixels).",                            1,      1,   500)
+gen.add("scan_offset",          double_t, 0,                                "Row set as the center of laserscan.",                              0.500,  0.0, 1.0)
 gen.add("scan_time",            double_t, 0,                                "Time for the entire scan sweep.",                                  0.033,  0.0, 1.0)
 gen.add("range_min",            double_t, 0,                                "Minimum reported range (in meters).",                              0.45,   0.0, 10.0)
 gen.add("range_max",            double_t, 0,                                "Maximum reported range (in meters).",                              10.0,   0.0, 10.0)

include/depthimage_to_laserscan/DepthImageToLaserScan.h

@@ -101,6 +101,17 @@ namespace depthimage_to_laserscan
      * 
      */
     void set_scan_height(const int scan_height);
+
+    /**
+     * Sets the center position of the output LaserScan.
+     * 
+     * scan_offset is the number between 0 and 1 that will be used to set center position in the output. 0 value corresponds
+     * to the bottom of the image while value 1 corresponds to the height of the image. 
+     * 
+     * @param scan_offset Center position of the LaserScan.
+     * 
+     */
+    void set_scan_offset(const float scan_offset);    
 
     /**
      * Sets the frame_id for the output LaserScan.
@@ -164,11 +175,12 @@ namespace depthimage_to_laserscan
     * @param cam_model The image_geometry camera model for this image.
     * @param scan_msg The output LaserScan.
     * @param scan_height The number of vertical pixels to feed into each angular_measurement.
+    * @param scan_offset The center of the output LaserScan.
     * 
     */
     template<typename T>
     void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, 
-		 const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{
+		 const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height, const float& scan_offset) const{
       // Use correct principal point from calibration
       float center_x = cam_model.cx();
       float center_y = cam_model.cy();
@@ -181,7 +193,7 @@ namespace depthimage_to_laserscan
       const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
       int row_step = depth_msg->step / sizeof(T);
 
-      int offset = (int)(cam_model.cy()-scan_height/2);
+      int offset = (int)((cam_model.cy()*2*scan_offset)-scan_height/2);
       depth_row += offset*row_step; // Offset to center of image
 
       for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){
@@ -216,10 +228,11 @@ namespace depthimage_to_laserscan
     float range_min_; ///< Stores the current minimum range to use.
     float range_max_; ///< Stores the current maximum range to use.
     int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area.
+    float scan_offset_; ///< Number of row being set as center of a laserscan.
     std::string output_frame_id_; ///< Output frame_id for each laserscan.  This is likely NOT the camera's frame_id.
   };
 
 
 }; // depthimage_to_laserscan
 
-#endif
+#endif

src/DepthImageToLaserScan.cpp

@@ -115,8 +115,12 @@ sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::
   scan_msg->range_min = range_min_;
   scan_msg->range_max = range_max_;
 
+  double center_row = cam_model_.cy()*2*scan_offset_;
+  double bottom_row = center_row - scan_height_/2;
+  double top_row = center_row + scan_height_/2;
+
   // Check scan_height vs image_height
-  if(scan_height_/2 > cam_model_.cy() || scan_height_/2 > depth_msg->height - cam_model_.cy()){
+  if(bottom_row < 0 || top_row >= depth_msg->height){
     std::stringstream ss;
     ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height.";
     throw std::runtime_error(ss.str());
@@ -128,11 +132,11 @@ sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::
 
   if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
   {
-    convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_);
+    convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_, scan_offset_);
   }
   else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
   {
-    convert<float>(depth_msg, cam_model_, scan_msg, scan_height_);
+    convert<float>(depth_msg, cam_model_, scan_msg, scan_height_, scan_offset_);
   }
   else
   {
@@ -157,6 +161,10 @@ void DepthImageToLaserScan::set_scan_height(const int scan_height){
   scan_height_ = scan_height;
 }
 
+void DepthImageToLaserScan::set_scan_offset(const float scan_offset){
+  scan_offset_ = scan_offset;
+}
+
 void DepthImageToLaserScan::set_output_frame(const std::string output_frame_id){
   output_frame_id_ = output_frame_id;
 }

src/DepthImageToLaserScanROS.cpp

@@ -87,5 +87,6 @@ void DepthImageToLaserScanROS::reconfigureCb(depthimage_to_laserscan::DepthConfi
     dtl_.set_scan_time(config.scan_time);
     dtl_.set_range_limits(config.range_min, config.range_max);
     dtl_.set_scan_height(config.scan_height);
+    dtl_.set_scan_offset(config.scan_offset);
     dtl_.set_output_frame(config.output_frame_id);
 }

test/DepthImageToLaserScanTest.cpp

@@ -61,6 +61,8 @@ TEST(ConvertTest, setupLibrary)
   dtl_.set_range_limits(range_min, range_max);
   const int scan_height = 1;
   dtl_.set_scan_height(scan_height);
+  const float scan_offset = 0.5;
+  dtl_.set_scan_offset(scan_offset);
   const std::string output_frame = "camera_depth_frame";
   dtl_.set_output_frame(output_frame);
 
@@ -282,4 +284,4 @@ TEST(ConvertTest, testNegativeInf)
 int main(int argc, char **argv){
 testing::InitGoogleTest(&argc, argv);
 return RUN_ALL_TESTS();
-}
+}