[bug fix] correct stereo_s depth image

.gitignore

@@ -0,0 +1 @@
+.vscode/

cfg/Astra.cfg

@@ -18,7 +18,8 @@ output_mode_enum = gen.enum([  gen.const(  "SXGA_30Hz", int_t, 1,  "1280x1024@30
                                gen.const(  "QVGA_60Hz", int_t, 9,  "320x240@60Hz"),
                                gen.const( "QQVGA_25Hz", int_t, 10, "160x120@25Hz"),
                                gen.const( "QQVGA_30Hz", int_t, 11, "160x120@30Hz"),
-                               gen.const( "QQVGA_60Hz", int_t, 12, "160x120@60Hz")],
+                               gen.const( "QQVGA_60Hz", int_t, 12, "160x120@60Hz"),
+                               gen.const("640400_30Hz", int_t, 13, "640x400@30Hz")],
                                "output mode")
 
 video_stream_enum = gen.enum([  gen.const(  "RGB", bool_t, True,  "RGB video stream preferred"),
@@ -27,9 +28,9 @@ video_stream_enum = gen.enum([  gen.const(  "RGB", bool_t, True,  "RGB video str
 
 gen.add("rgb_preferred", bool_t, 0, "Preferred camera stream", True, edit_method = video_stream_enum)
 
-gen.add("ir_mode", int_t, 0, "Video mode for IR camera", 5, 1, 12, edit_method = output_mode_enum)
-gen.add("color_mode", int_t, 0, "Video mode for color camera", 5, 1, 12, edit_method = output_mode_enum)
-gen.add("depth_mode", int_t, 0, "Video mode for depth camera", 5, 1, 12, edit_method = output_mode_enum)
+gen.add("ir_mode", int_t, 0, "Video mode for IR camera", 5, 1, 13, edit_method = output_mode_enum)
+gen.add("color_mode", int_t, 0, "Video mode for color camera", 5, 1, 13, edit_method = output_mode_enum)
+gen.add("depth_mode", int_t, 0, "Video mode for depth camera", 5, 1, 13, edit_method = output_mode_enum)
 
 gen.add("depth_registration", bool_t, 0, "Depth data registration", True)
 gen.add("color_depth_synchronization", bool_t, 0, "Synchronization of color and depth camera", False)

include/astra_camera/astra_device.h

@@ -37,6 +37,8 @@
 
 #include "astra_camera/astra_exception.h"
 
+#include <openni2/OpenNI.h>
+
 #include <boost/shared_ptr.hpp>
 #include <boost/cstdint.hpp>
 #include <boost/bind.hpp>
@@ -124,6 +126,9 @@ class AstraDevice
   float getColorFocalLength (int output_y_resolution) const;
   float getDepthFocalLength (int output_y_resolution) const;
   float getBaseline () const;
+  OBCameraParams getIntrParams() const;
+  char* getSerialNumber();
+  char* getDeviceType();
 
   void setAutoExposure(bool enable) throw (AstraException);
   void setAutoWhiteBalance(bool enable) throw (AstraException);
@@ -163,6 +168,9 @@ class AstraDevice
 
   bool use_device_time_;
 
+  OBCameraParams m_CamParams;
+  char serial_number[12];
+  char device_type[32];
 };
 
 std::ostream& operator << (std::ostream& stream, const AstraDevice& device);

include/openni2_redist/x64/OpenNI2/Drivers/orbbec.ini

@@ -41,10 +41,10 @@ Resolution=17
 ;FPS=30
 
 ; Min depth cutoff. 0-10000 mm (default is 0)
-MinDepthValue=280
+;MinDepthValue=280
 
 ; Max depth cutoff. 0-10000 mm (default is 10000)
-MaxDepthValue=3000
+;MaxDepthValue=3000
 
 ; Input format. 0 - Uncompressed 16-bit, 1 - PS Compression, 3 - Packed 11-bit, 4 - Packed 12-bit. Default: Arm - 4, other platforms - 3
 InputFormat=3

package.xml

@@ -6,7 +6,7 @@
   <author email="[email protected]">Tim Liu</author>
   <license>BSD</license>
 
-  <maintainer email="liuhua@orbbec.com">Tim Liu</maintainer>
+  <maintainer email="cjwu@orbbec3d.com">Sayter99</maintainer>
 
   <buildtool_depend>catkin</buildtool_depend>
 

src/astra_convert.cpp

@@ -71,7 +71,6 @@ const AstraVideoMode astra_convert(const openni::VideoMode& input)
 
 const openni::VideoMode astra_convert(const AstraVideoMode& input)
 {
-
   openni::VideoMode output;
 
   output.setResolution(input.x_resolution_, input.y_resolution_);

src/astra_device.cpp

@@ -78,6 +78,17 @@ AstraDevice::AstraDevice(const std::string& device_URI) throw (AstraException) :
   device_info_ = boost::make_shared<openni::DeviceInfo>();
   *device_info_ = openni_device_->getDeviceInfo();
 
+  int param_size = sizeof(OBCameraParams);
+  openni_device_->getProperty(openni::OBEXTENSION_ID_CAM_PARAMS, (uint8_t*)&m_CamParams, &param_size);
+
+  int serial_number_size = sizeof(serial_number);
+  memset(serial_number, 0, serial_number_size);
+  openni_device_->getProperty(openni::OBEXTENSION_ID_SERIALNUMBER, (uint8_t*)&serial_number, &serial_number_size);
+
+  int device_type_size = sizeof(device_type);
+  memset(device_type, 0, device_type_size);
+  openni_device_->getProperty(openni::OBEXTENSION_ID_DEVICETYPE, (uint8_t*)&device_type, &device_type_size);
+
   ir_frame_listener = boost::make_shared<AstraFrameListener>();
   color_frame_listener = boost::make_shared<AstraFrameListener>();
   depth_frame_listener = boost::make_shared<AstraFrameListener>();
@@ -134,6 +145,21 @@ bool AstraDevice::isValid() const
   return (openni_device_.get() != 0) && openni_device_->isValid();
 }
 
+OBCameraParams AstraDevice::getIntrParams() const
+{
+  return m_CamParams;
+}
+
+char* AstraDevice::getSerialNumber()
+{
+  return serial_number;
+}
+
+char* AstraDevice::getDeviceType()
+{
+  return device_type;
+}
+
 float AstraDevice::getIRFocalLength(int output_y_resolution) const
 {
   float focal_length = 0.0f;

src/astra_driver.cpp

@@ -150,7 +150,6 @@ AstraDriver::AstraDriver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
   ROS_DEBUG("Dynamic reconfigure configuration received.");
 
   advertiseROSTopics();
-
 }
 
 void AstraDriver::advertiseROSTopics()
@@ -230,6 +229,11 @@ bool AstraDriver::getSerialCb(astra_camera::GetSerialRequest& req, astra_camera:
 
 void AstraDriver::configCb(Config &config, uint32_t level)
 {
+  if (strcmp(device_->getDeviceType(), "Orbbec Canglong") == 0)
+  {
+    config.depth_mode = 13;
+    config.ir_mode = 13;
+  }
   bool stream_reset = false;
 
   rgb_preferred_ = config.rgb_preferred;
@@ -653,6 +657,14 @@ sensor_msgs::CameraInfoPtr AstraDriver::getIRCameraInfo(int width, int height, r
   {
     // If uncalibrated, fill in default values
     info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(height));
+    if (strcmp(device_->getDeviceType(), "Orbbec Canglong") == 0)
+    {
+      OBCameraParams p = device_->getIntrParams();
+      info->P[0] = p.l_intr_p[0];
+      info->P[5] = p.l_intr_p[1];
+      info->K[0] = p.l_intr_p[0];
+      info->K[5] = p.l_intr_p[1];
+    }
   }
 
   // Fill in header
@@ -907,7 +919,8 @@ output_mode_enum = gen.enum([  gen.const(  "SXGA_30Hz", int_t, 1,  "1280x1024@30
                                gen.const(  "QVGA_60Hz", int_t, 9,  "320x240@60Hz"),
                                gen.const( "QQVGA_25Hz", int_t, 10, "160x120@25Hz"),
                                gen.const( "QQVGA_30Hz", int_t, 11, "160x120@30Hz"),
-                               gen.const( "QQVGA_60Hz", int_t, 12, "160x120@60Hz")],
+                               gen.const( "QQVGA_60Hz", int_t, 12, "160x120@60Hz"),
+                               gen.const("640400_30Hz", int_t, 13, "640x400@30Hz")],
                                "output mode")
   */