Updated depth correction nodelet to use get serial service and minor …

…changes to documentation

rgbd_depth_correction/CMakeLists.txt

@@ -16,7 +16,6 @@ find_package(catkin REQUIRED COMPONENTS
   std_srvs
   target_finder
   tf
-  tf2_bullet
   tf_conversions
 )
 
@@ -47,10 +46,6 @@ find_library(
   PATHS ${yaml_cpp_LIBRARY_DIRS}
 )
 
-#message("yaml_cpp_INCLUDE_DIRS: ${yaml_cpp_INCLUDE_DIRS}")
-#message("yaml_cpp_INCLUDE_DIR: ${yaml_cpp_INCLUDE_DIR}")
-#message("yaml_cpp_LIBRARY: ${yaml_cpp_LIBRARY}")
-
 
 catkin_package(
   CATKIN_DEPENDS
@@ -68,7 +63,6 @@ catkin_package(
     std_srvs
     target_finder
     tf
-    tf2_bullet
     tf_conversions
   DEPENDS
     Boost

rgbd_depth_correction/include/depth_calibration/depth_calibration.h

@@ -197,6 +197,7 @@ class DepthCalibrator
   geometry_msgs::Pose target_initial_pose_;  /**< @brief The initial pose guess of the calibration target for the target finder service */
 
   boost::mutex data_lock_; /**< @brief Lock for data subscription */
+  sensor_msgs::Image last_image_;
   pcl::PointCloud<pcl::PointXYZ> last_cloud_;  /**< @brief The last point cloud received */
   pcl::PointCloud<pcl::PointXYZ> correction_cloud_;  /**< @brief The point cloud containing the depth correction values */
   std::vector< pcl::PointCloud<pcl::PointXYZ>, Eigen::aligned_allocator<pcl::PointCloud<pcl::PointXYZ> > > saved_clouds_;
@@ -231,6 +232,7 @@ class DepthCalibrator
      */
   bool findAveragePlane(std::vector<double> &plane_eq, geometry_msgs::Pose& target_pose);
 
+  bool findAveragePointCloud(pcl::PointCloud<pcl::PointXYZ>& final_cloud);
 };
 
 #endif // DEPTH_CALIBRATION_H

rgbd_depth_correction/package.xml

@@ -30,7 +30,6 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>target_finder</build_depend>
-  <build_depend>tf2_bullet</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>tf_conversions</build_depend>
   <build_depend>target_finder</build_depend>
@@ -55,7 +54,6 @@
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>target_finder</run_depend>
-  <run_depend>tf2_bullet</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>tf_conversions</run_depend>
   <run_depend>yaml-cpp</run_depend>

rgbd_depth_correction/src/depth_calibration.cpp

@@ -125,7 +125,7 @@ bool DepthCalibrator::calibrateCameraDepth(std_srvs::Empty::Request &request, st
   {
     if( !(saved_clouds_[0].points.size() == correction_cloud_.points.size()) )
     {
-      ROS_ERROR("Point cloud pixel depth correction cloud size (%d) not the same size as saved cloud size (%d). Aborting depth calibration.",
+      ROS_ERROR("Point cloud pixel depth correction cloud size (%lu) not the same size as saved cloud size (%lu). Aborting depth calibration.",
                 correction_cloud_.points.size(), saved_clouds_[0].points.size());
       return false;
     }
@@ -199,7 +199,7 @@ bool DepthCalibrator::findAveragePointCloud(pcl::PointCloud<pcl::PointXYZ>& fina
         final_cloud = last_cloud_;
         if(ros_cloud_header.stamp > new_time)
         {
-          ROS_INFO("Got point cloud %d of %d", (temp_clouds.size() + 1), num_point_clouds_ );
+          ROS_INFO("Got point cloud %lu of %d", (temp_clouds.size() + 1), num_point_clouds_ );
           temp_clouds.push_back(last_cloud_);
           break;
         }

rgbd_depth_correction/src/depth_correction.cpp

@@ -29,6 +29,7 @@
 #include <nodelet/nodelet.h>
 
 #include <industrial_extrinsic_cal/yaml_utils.h>
+#include <openni2_camera/GetSerial.h>
 
 
 namespace rgbd_depth_correction{
@@ -41,7 +42,7 @@ class DepthCorrectionNodelet : public nodelet::Nodelet
   double d1_, d2_;      /**< @brief The depth coefficients */
 
   int version_;  /**< @brief The version number found in the YAML file */
-  pcl::PointCloud<pcl::PointXYZRGB> correction_cloud_;  /**< @brief The depth correction point cloud containing the depth correction values */
+  pcl::PointCloud<pcl::PointXYZ> correction_cloud_;  /**< @brief The depth correction point cloud containing the depth correction values */
 
   ros::Subscriber pcl_sub_;          /**< @brief PCL point cloud subscriber */
   ros::Publisher pcl_pub_;           /**< @brief PCL point cloud publisher for the corrected point cloud */
@@ -52,7 +53,7 @@ class DepthCorrectionNodelet : public nodelet::Nodelet
      *
      * @param[in] cloud Latest point cloud received
      */
-  void pointcloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud);
+  void pointcloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud);
 
   /**
      * @brief Give a pathway and file name, reads the version number and loads the appropriate depth correction parameters
@@ -75,7 +76,7 @@ class DepthCorrectionNodelet : public nodelet::Nodelet
      *
      * @param[in] cloud The point cloud to be corrected and republished
      */
-  void correctionVersionOne(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud);
+  void correctionVersionOne(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud);
 
 public:
 
@@ -102,15 +103,44 @@ class DepthCorrectionNodelet : public nodelet::Nodelet
     ros::NodeHandle nh = getMTNodeHandle();
     ros::NodeHandle priv_nh = getMTPrivateNodeHandle();
 
-    // get parameters for services, topics, and filename
+    ros::ServiceClient get_serial_no = nh.serviceClient<openni2_camera::GetSerial>("get_serial");
+    openni2_camera::GetSerial msg;
+
     std::string filename, filepath;
-    if(!priv_nh.getParam("filename", filename) || !priv_nh.getParam("filepath", filepath))
+
+    if(!priv_nh.getParam("filepath", filepath))
+    {
+      ROS_ERROR("File pathway not provided.  Closing depth correction node.");
+      return;
+    }
+
+    // Check for filename argument first, use 'get_serial' service if argument is not provided
+    if(priv_nh.getParam("filename", filename))
+    {
+      ROS_INFO("Using provided camera file name argument '%s' for depth correction", filename.c_str());
+    }
+    else if(get_serial_no.waitForExistence(ros::Duration(30.0)))
+    {
+      if(get_serial_no.call(msg.request, msg.response))
+      {
+        filename = msg.response.serial;
+        ROS_INFO("Using camera serial number '%s' from camera driver service for depth correction", filename.c_str());
+      }
+      else
+      {
+        ROS_ERROR("Camera get serial number service not available.  Closing depth correction node.");
+        return;
+      }
+    }
+    else
     {
-      ROS_ERROR("File name and/or pathway not provided.  Closing depth correction node.");
+      ROS_ERROR("Cannot open configuration files because argument 'filename' not provided and camera 'get_serial' service not available. Closing depth correction node.");
       return;
     }
 
-    ROS_INFO_STREAM("Reading in yaml file " << filepath << filename);
+    // get parameters for services, topics, and filename
+
+    ROS_INFO_STREAM("Reading in yaml file " << filepath << filename << ".yaml");
 
     if(!readYamlFile(filepath, filename))
     {
@@ -120,15 +150,15 @@ class DepthCorrectionNodelet : public nodelet::Nodelet
 
     ROS_INFO("Done reading yaml file");
 
-    pcl_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("out_cloud",1);
+    pcl_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("out_cloud",1);
     pcl_sub_ = nh.subscribe("in_cloud", 1, &DepthCorrectionNodelet::pointcloudCallback, this);
 
     depth_change_ = nh.advertiseService("change_depth_factor", &DepthCorrectionNodelet::setEnableDepth, this);
 
   }
 };
 
-void DepthCorrectionNodelet::pointcloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
+void DepthCorrectionNodelet::pointcloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
 {
   switch(version_)
   {
@@ -152,23 +182,23 @@ bool DepthCorrectionNodelet::readYamlFile(const std::string &pathway, const std:
     rtn = false;
   }
   else if(!industrial_extrinsic_cal::parseInt(doc, "version", version_))
-    {
-      ROS_ERROR("Yaml file did not contain depth correction version information");
-      rtn = false;
-    }
+  {
+    ROS_ERROR("Yaml file did not contain depth correction version information");
+    rtn = false;
+  }
   else
+  {
+    switch (version_)
     {
-      switch (version_)
-        {
-	case 1:
-	  loadVersionOne(doc, pathway + yaml_file);
-	  break;
-	default:
-	  ROS_ERROR("Version in YAML file did not match any known versions");
-	  rtn = false;
-	  break;
-        }
+      case 1:
+        loadVersionOne(doc, pathway + yaml_file);
+        break;
+      default:
+        ROS_ERROR("Version in YAML file did not match any known versions");
+        rtn = false;
+        break;
     }
+  }
   return rtn;
 }
 
@@ -192,9 +222,9 @@ void DepthCorrectionNodelet::loadVersionOne(const YAML::Node &doc, const std::st
   pcl::io::loadPCDFile(pcd_file, correction_cloud_);
 }
 
-void DepthCorrectionNodelet::correctionVersionOne(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud)
+void DepthCorrectionNodelet::correctionVersionOne(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
 {
-  pcl::PointCloud<pcl::PointXYZRGB> corrected_cloud = *cloud;
+  pcl::PointCloud<pcl::PointXYZ> corrected_cloud = *cloud;
   if(correction_cloud_.points.size() == cloud->points.size())
   {
     for(int i = 0; i < corrected_cloud.points.size(); ++i)
@@ -214,8 +244,13 @@ void DepthCorrectionNodelet::correctionVersionOne(const pcl::PointCloud<pcl::Poi
       }
     }
   }
+  else
+  {
+    ROS_ERROR_STREAM_THROTTLE(30, "Depth correction cloud size and input point cloud size do not match.  Not performing depth correction");
+  }
 
-  pcl_pub_.publish(corrected_cloud);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr published_cloud = corrected_cloud.makeShared();
+  pcl_pub_.publish(published_cloud);
 }
 
 PLUGINLIB_DECLARE_CLASS(rgbd_depth_correction, DepthCorrectionNodelet, rgbd_depth_correction::DepthCorrectionNodelet, nodelet::Nodelet);