[add] ldp switch

1. updage openni
2. update readme
3. add ldp service

CMakeLists.txt

@@ -53,6 +53,7 @@ add_service_files(
   SetIRGain.srv
   SetIRFlood.srv
   SetLaser.srv
+  SetLDP.srv
   SetUVCExposure.srv
   ResetIRGain.srv
   ResetIRExposure.srv

README.md

@@ -84,6 +84,7 @@ This package provides multiple [ros services](http://wiki.ros.org/Services) for
 * `/camera/set_ir_exposure`: set ir exposure to specific value
 * `/camera/set_ir_gain`: set ir gain to specific value
 * `/camera/set_laser`: turn on (**true**) or turn off (**false**) laser
+* `/camera/set_ldp`: turn on (**true**) or turn off (**false**) ldp
 * `/camera/set_uvc_exposure`: set uvc exposure. (set **0** indicating auto mode)
 * `/camera/set_uvc_gain`: set uvc gain
 * `/camera/set_uvc_white_balance`: set uvc white balance (set **0** indicating auto mode)

include/astra_camera/astra_device.h

@@ -140,6 +140,7 @@ class AstraDevice
   void setIRExposure(int exposure);
   void setLaser(bool enable);
   void setIRFlood(bool enable);
+  void setLDP(bool enable);
 
   void switchIRCamera(int cam);
 

include/astra_camera/astra_driver.h

@@ -59,6 +59,7 @@
 #include "astra_camera/GetIRExposure.h"
 #include "astra_camera/SetIRExposure.h"
 #include "astra_camera/SetLaser.h"
+#include "astra_camera/SetLDP.h"
 #include "astra_camera/ResetIRGain.h"
 #include "astra_camera/ResetIRExposure.h"
 #include "astra_camera/GetCameraInfo.h"
@@ -116,6 +117,7 @@ class AstraDriver
   bool getCameraInfoCb(astra_camera::GetCameraInfoRequest& req, astra_camera::GetCameraInfoResponse& res);
   bool setIRFloodCb(astra_camera::SetIRFloodRequest& req, astra_camera::SetIRFloodResponse& res);
   bool switchIRCameraCb(astra_camera::SwitchIRCameraRequest& req, astra_camera::SwitchIRCameraResponse& res);
+  bool setLDPCb(astra_camera::SetLDPRequest& req, astra_camera::SetLDPResponse& res);
 
   void configCb(Config &config, uint32_t level);
 
@@ -148,6 +150,7 @@ class AstraDriver
   ros::ServiceServer set_ir_exposure_server;
   ros::ServiceServer set_ir_flood_server;
   ros::ServiceServer set_laser_server;
+  ros::ServiceServer set_ldp_server;
   ros::ServiceServer reset_ir_gain_server;
   ros::ServiceServer reset_ir_exposure_server;
   ros::ServiceServer switch_ir_camera;

include/openni2/PS1080.h

@@ -157,7 +157,34 @@ enum
 	//rgb Ae mode
 	XN_MODULE_PROPERTY_RGB_AE_MODE = 0x1080FFB1,
 
-
+	//IR Temperature mode
+	XN_MODULE_PROPERTY_CAL_IR_TEMP = 0x1080FFB2,
+	//LDMP Temperature mode
+	XN_MODULE_PROPERTY_CAL_LDMP_TEMP = 0x1080FFB3,
+	//IR real time Temperature mode
+	XN_MODULE_PROPERTY_RT_IR_TEMP = 0x1080FFB4,
+	//LDMP real time Temperature mode
+	XN_MODULE_PROPERTY_RT_LDMP_TEMP = 0x1080FFB5,
+	//IR temperature compensation coefficient mode
+	XN_MODULE_PROPERTY_IR_TEMP_COMP_CO = 0x1080FFB6,
+	//Ldmp temperature compensation coefficient mode
+	XN_MODULE_PROPERTY_LDMP_TEMP_COMP_CO = 0x1080FFB7,
+
+	//Temperature comp
+	XN_MODULE_PROPERTY_TEMP_COMP = 0x1080FFB8,
+
+	//Depth optimization enabled state
+	XN_MODULE_PROPERTY_DEPTH_OPTIM_STATE = 0x1080FFB9,
+
+	//Depth optimization param
+	XN_MODULE_PROPERTY_DEPTH_OPTIM_PARAM = 0x1080FFBA,
+
+	XN_MODULE_PROPERTY_UPDATE_FIRMWARE_FLASH_CHUNK = 0x1080FFBD,
+
+	XN_MODULE_PROPERTY_LDP_ENABLE = 0x1080FFBE,
+        XN_MODULE_PROPERTY_EMITTER_STATE_V1 = 0x1080FFBF, // "get Emitter enable"
+	XN_MODULE_PROPERTY_LDP_SCALE = 0x1080FFC0,       //ldp scale
+	XN_MODULE_PROPERTY_LDP_STATUS = 0x1080FFC1,      //ldp status
 	/*******************************************************************/
 	/* Common stream properties                                        */
 	/*******************************************************************/
@@ -221,6 +248,9 @@ enum
 	XN_STREAM_PROPERTY_DEPTH_SENSOR_CALIBRATION_INFO = 0x10801012,
 	/** Boolean */
 	XN_STREAM_PROPERTY_GMC_MODE	= 0x1080FF44, // "GmcMode"
+
+        XN_STREAM_PROPERTY_MIN_DEPTH  = 0x1080FF40, // "MinDepthValue"
+        XN_STREAM_PROPERTY_MAX_DEPTH  = 0x1080FF41, // "MaxDepthValue"
 	/** Boolean */
 	XN_STREAM_PROPERTY_GMC_DEBUG = 0x1080FF45, // "GmcDebug"
 	/** Boolean */
@@ -230,6 +260,7 @@ enum
 
     /** Boolean */
     XN_STREAM_PROPERTY_SOFTWARE_REGISTRATION = 0x2080FF42, // "Software Registration"
+	XN_STREAM_PROPERTY_SOFTWARE_FILTER = 0x2080FF43,		//soft filter enable
 
 	/*******************************************************************/
 	/* Color stream properties                                         */
@@ -291,7 +322,7 @@ typedef enum
 {
 	XN_CMOS_TYPE_IMAGE = 0,
 	XN_CMOS_TYPE_DEPTH = 1,
-
+	XN_CMOS_TYPE_IR = 2,
 	XN_CMOS_COUNT
 } XnCMOSType;
 
@@ -708,6 +739,14 @@ typedef struct XnRgbAeMode
 	uint16_t nAeTarget;
 } XnRgbAeMode;
 
+typedef struct XnDepthOptimizationParam
+{
+	double nParam1;
+	double nParam2;
+	double nParam3;
+}XnDepthOptimizationParam;
+
+
 #pragma pack (pop)
 
-#endif // PS1080_H
+#endif // PS1080_H

src/astra_device.cpp

@@ -255,6 +255,30 @@ void AstraDevice::setIRFlood(bool enable)
   openni_device_->setProperty(XN_MODULE_PROPERTY_IRFLOOD_STATE, enable_);
 }
 
+void AstraDevice::setLDP(bool enable)
+{
+  int data_size = 4;
+  int enable_ = 1;
+  if (enable == false)
+  {
+    enable_ = 0;
+  }
+  if (device_type_no == OB_STEREO_S_U3_NO)
+  {
+    openni_device_->setProperty(XN_MODULE_PROPERTY_LDP_ENABLE, (uint8_t *)&enable_, 4);
+  }
+  else
+  {
+    boost::shared_ptr<openni::VideoStream> depth_stream = getDepthVideoStream();
+    boost::shared_ptr<openni::VideoStream> ir_stream = getIRVideoStream();
+    depth_stream->stop();
+    ir_stream->stop();
+    openni_device_->setProperty(openni::OBEXTENSION_ID_LDP_EN, (uint8_t *)&enable_, 4);
+    depth_stream->start();
+    ir_stream->start();
+  }
+}
+
 void AstraDevice::switchIRCamera(int cam)
 {
   if (device_type_no == OB_STEREO_S_NO || device_type_no == OB_STEREO_S_U3_NO)

src/astra_driver.cpp

@@ -232,6 +232,7 @@ void AstraDriver::advertiseROSTopics()
   set_ir_exposure_server = nh_.advertiseService("set_ir_exposure", &AstraDriver::setIRExposureCb, this);
   set_ir_flood_server = nh_.advertiseService("set_ir_flood", &AstraDriver::setIRFloodCb, this);
   set_laser_server = nh_.advertiseService("set_laser", &AstraDriver::setLaserCb, this);
+  set_ldp_server = nh_.advertiseService("set_ldp", &AstraDriver::setLDPCb, this);
   reset_ir_gain_server = nh_.advertiseService("reset_ir_gain", &AstraDriver::resetIRGainCb, this);
   reset_ir_exposure_server = nh_.advertiseService("reset_ir_exposure", &AstraDriver::resetIRExposureCb, this);
   get_camera_info = nh_.advertiseService("get_camera_info", &AstraDriver::getCameraInfoCb, this);
@@ -283,6 +284,12 @@ bool AstraDriver::setLaserCb(astra_camera::SetLaserRequest& req, astra_camera::S
   return true;
 }
 
+bool AstraDriver::setLDPCb(astra_camera::SetLDPRequest& req, astra_camera::SetLDPResponse& res)
+{
+  device_->setLDP(req.enable);
+  return true;
+}
+
 bool AstraDriver::resetIRGainCb(astra_camera::ResetIRGainRequest& req, astra_camera::ResetIRGainResponse& res)
 {
   device_->setIRGain(0x8);

srv/SetLDP.srv

@@ -0,0 +1,2 @@
+bool enable
+---