feature(): capture RGBD service

Author: Daniel Claes

.gitignore

@@ -2,3 +2,4 @@ build
 BUILD
 Build
 *.user
+*.idea

CMakeLists.txt

@@ -39,6 +39,7 @@ find_package(
     catkin REQUIRED
     COMPONENTS
 	sr_foundation
+        message_generation
         rgbd
         roscpp
         rospy
@@ -68,6 +69,19 @@ message(STATUS "Ensenso Libraries found at ${ENSENSO_LIBRARIES}.")
 # add other necessary include directories
 include_directories(src/ ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
 
+## Generate services in the 'srv' folder
+add_service_files(
+        FILES
+        CaptureRGBD.srv
+)
+
+
+generate_messages(  DEPENDENCIES
+        std_msgs
+        rgbd
+        )
+
+
 # Set source files
 SET(SRCS_LIB
     src/ensenso_nx.cpp
@@ -104,9 +118,6 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME}
                       ${ENSENSO_LIBRARIES} ${OpenCV_LIBS} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${catkin_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
 
-#Build examples
-ADD_SUBDIRECTORY(src/examples)
-
 install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

src/ensenso_nx.cpp

@@ -2,7 +2,7 @@
 #include "ros/ros.h"
 
 void ensensoExceptionHandling (const NxLibException &ex,
-                               std::string func_nam)
+                               const std::string func_nam)
 {
     ROS_ERROR ("%s: NxLib error %s (%d) occurred while accessing item %s.\n", func_nam.c_str (), ex.getErrorText ().c_str (), ex.getErrorCode (),
                ex.getItemPath ().c_str ());
@@ -86,21 +86,6 @@ namespace EnsensoNx
         this->configureCapture();
     }
 
-// void Device::configureExposure(unsigned int _exposure)
-// {
-//     if (_exposure == 0) //autoexposure case
-//     {
-//         capture_params_.auto_exposure_ = true;
-//     }
-//     else //manual exposure case
-//     {
-//         capture_params_.auto_exposure_ = false;
-//         capture_params_.exposure_time_ = _exposure;
-//     }
-//
-//     //call protected member to set the configuration to the camera
-//     this->configureCapture();
-// }
     void Device::preCapture() {
         // Capture images
         try {
@@ -117,7 +102,7 @@ namespace EnsensoNx
     {
         try {
             // Capture images
-            //NxLibCommand (cmdCapture).execute();
+            NxLibCommand (cmdCapture).execute();
 
             // Compute Disparity Map
             NxLibCommand (cmdComputeDisparityMap).execute();
@@ -129,7 +114,7 @@ namespace EnsensoNx
             int width, height;
             double time_stamp;
             camera_[itmImages][itmPointMap].getBinaryDataInfo(&width, &height, nullptr,
-                                                              nullptr, nullptr, &time_stamp);
+                                                              nullptr, nullptr, nullptr);
             //std::cout << "ts1: " << std::fixed << time_stamp << std::endl;
             //Get 3D image raw data
             int nx_return_code;
@@ -177,13 +162,13 @@ namespace EnsensoNx
             int nx_return_code;
 
             // Capture images
-            //NxLibCommand(cmdCapture).execute();
+            NxLibCommand (cmdCapture).execute();
 
             // Compute Disparity Map
-            NxLibCommand(cmdComputeDisparityMap).execute();
+            NxLibCommand (cmdComputeDisparityMap).execute();
 
             // Compute Point Cloud
-            NxLibCommand(cmdComputePointMap).execute();
+            NxLibCommand (cmdComputePointMap).execute();
             double time_stamp;
             // Get image dimensions
             camera_[itmImages][itmPointMap].getBinaryDataInfo(&ww, &hh, 0, 0, 0, &time_stamp);
@@ -227,14 +212,6 @@ namespace EnsensoNx
                     kk);//checks if kk=ww*hh to set the cloud as ordered (width,height) or unordered (width=size,height=1)
             _p_cloud.is_dense = capture_params_.dense_cloud_;
 
-            //debug message
-//     std::cout << "Cloud capture: " << std::endl <<
-//                  "\treturn code: " << nx_return_code << std::endl <<
-//                  "\tnum points: " << raw_points_.size()/3 << std::endl <<
-//                  "\twidth: " << ww << std::endl <<
-//                  "\theight: " << hh << std::endl <<
-//                  "\tvalid_points: " << kk << std::endl;
-
             //return success
             return 1;
         }
@@ -245,17 +222,13 @@ namespace EnsensoNx
         }
     }
 
-//PROTECTED METHODS
 
     void Device::configureCapture()
     {
         //sets capture configuration to the camera
         camera_[itmParameters][itmCapture][itmAutoExposure] = capture_params_.auto_exposure_;
         camera_[itmParameters][itmCapture][itmExposure    ] = (double)capture_params_.exposure_time_;//TODO check if requires cast to double.
 
-        //print out
-        //std::cout << "EnsensoNx::Device: Capture params set to:" << std::endl;
-        //capture_params_.print();
     }
 
 }//close namespace

src/ensenso_nx_node.cpp

@@ -88,11 +88,6 @@ bool EnsensoNxNode::publishRGBD()
     //Get a single capture from camera and publish the point cloud
     if ( camera_->capture(image_) == 1 )
     {
-        //get time
-        ros::Time ts = ros::Time::now();
-
-        //publish the cloud
-        //image_.timestamp = (pcl::uint64_t)(ts.toSec()*1e9); //TODO: should be set by the EnsensoNx::Device class
         image_.frame_id = frame_name_;
         sr::rgbd::toData(image_, rgbd_msg.data);
 
@@ -124,24 +119,30 @@ bool EnsensoNxNode::publishCloud()
     return false;
 }
 
-bool EnsensoNxNode::publishServiceCallback(std_srvs::Trigger::Request &_request,
-                                           std_srvs::Trigger::Response &_reply)
+bool EnsensoNxNode::publishServiceCallback(ensenso_nx::CaptureRGBD::Request &_request,
+                                           ensenso_nx::CaptureRGBD::Response &_reply)
 {
+    if (_request.exposure == 0) {
+        capture_params_.auto_exposure_ = true;
+        capture_params_.exposure_time_ = 0;
+    }
+    else {
+        capture_params_.auto_exposure_ = false;
+        capture_params_.exposure_time_ = _request.exposure;
+
+    }
+
+    camera_->configureCapture(capture_params_);
+
+    if ( camera_->capture(image_) == 1 ) {
+        image_.frame_id = frame_name_;
+        sr::rgbd::toData(image_, rgbd_msg.data);
+        _reply.rgbd_image = rgbd_msg;
+        _reply.success = true;
+    }
+    else {
+        _reply.success = false;
+    }
 
-//    capture_params_.auto_exposure_ = true;
-//    capture_params_.dense_cloud_ = true;
-//    camera_->configureCapture(capture_params_);
-//    
-    do_publish = true;
-//    if (this->publish()){
-//        _reply.message = "Successfully published.";
-//        _reply.success = true;
-//    }
-//    else {
-//        _reply.message = "Failed to publish.";
-//        _reply.success = false;
-//    }
-
-    //return
     return true;
 }

src/ensenso_nx_node.h

@@ -19,6 +19,7 @@
 
 //this package dependencies
 #include <ensenso_nx/ensenso_nx_paramsConfig.h> //ROS dynamic configure
+#include <ensenso_nx/CaptureRGBD.h>
 
 //enum run mode
 enum RunMode {SERVER=0,PUBLISHER};
@@ -92,8 +93,8 @@ class EnsensoNxNode
 
 protected:
         //Service callback implementing the point cloud capture
-        bool publishServiceCallback(std_srvs::Trigger::Request &_request,
-                                    std_srvs::Trigger::Response &_reply);
+        bool publishServiceCallback(ensenso_nx::CaptureRGBD::Request &_request,
+                                    ensenso_nx::CaptureRGBD::Response &_reply);
 
 };
 #endif

src/ensenso_nx_node_main.cpp

@@ -17,13 +17,9 @@ int main(int argc, char **argv)
       //node loop 
       while ( ros::ok() )
       {
-            ensenso.preCapture();
             //execute pending callbacks
             ros::spinOnce(); 
-            if (ensenso.do_publish) {
-                ensenso.publish();
-                ensenso.do_publish = false;
-            }
+
             //switch according run mode
             switch(ensenso.runMode())
             {

src/examples/CMakeLists.txt

@@ -0,0 +1,10 @@
+#Request/Reply for RGBDImage
+
+# Request:
+#   exposure in milliseconds (0 indicates autoexposure) 
+uint32 exposure
+---
+
+# Reply with the size of the captured rgbd_image
+rgbd/RGBDImage rgbd_image
+bool success