added depthmap publisher

uzh-rpg · Nov 13, 2015 · d252baa · d252baa
1 parent a9aa0fa
commit d252baa
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 11 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -53,10 +53,10 @@ find_package(Boost REQUIRED COMPONENTS system filesystem)
 
 if(BUILD_ROS_NODE)
   find_package(catkin REQUIRED COMPONENTS
-    roscpp roslib cmake_modules svo_msgs cv_bridge vikit_ros)
+    roscpp roslib cmake_modules svo_msgs cv_bridge image_transport vikit_ros)
   catkin_package(
       DEPENDS OpenCV Eigen Boost
-      CATKIN_DEPENDS roscpp roslib svo_msgs vikit_ros
+      CATKIN_DEPENDS roscpp roslib svo_msgs image_transport vikit_ros
       INCLUDE_DIRS include
       LIBRARIES open_remode open_remode_cuda
   )

diff --git a/include/rmd/depthmap_node.h b/include/rmd/depthmap_node.h
@@ -18,10 +18,12 @@
 #ifndef RMD_DEPTHMAP_NODE_H
 #define RMD_DEPTHMAP_NODE_H
 
-#include <svo_msgs/DenseInput.h>
-
 #include <rmd/depthmap.h>
 
+#include <svo_msgs/DenseInput.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+
 namespace rmd
 {
 
@@ -38,14 +40,17 @@ typedef ProcessingStates::State State;
 class DepthmapNode
 {
 public:
-  DepthmapNode();
+  DepthmapNode(ros::NodeHandle &nh);
   bool init();
   void denseInputCallback(
       const svo_msgs::DenseInputConstPtr &dense_input);
 private:
   std::unique_ptr<rmd::Depthmap> depthmap_;
   State state_;
   float ref_compl_perc_;
+
+  ros::NodeHandle &nh_;
+  image_transport::Publisher depthmap_publisher_;
 };
 
 } // rmd namespace

diff --git a/package.xml b/package.xml
@@ -18,13 +18,15 @@
   <build_depend>roslib</build_depend>
   <build_depend>svo_msgs</build_depend>
   <build_depend>cv_bridge</build_depend>
+  <build_depend>image_transport</build_depend>
   <build_depend>vikit_ros</build_depend>
 
   <!-- Dependencies needed after this package is compiled. -->
   <run_depend>roscpp</run_depend>
   <run_depend>roslib</run_depend>
   <run_depend>svo_msgs</run_depend>
   <run_depend>cv_bridge</run_depend>
+  <run_depend>image_transport</run_depend>
   <run_depend>vikit_ros</run_depend>
 
 </package>
diff --git a/src/depthmap_node.cpp b/src/depthmap_node.cpp
@@ -24,9 +24,12 @@
 #include <rmd/depthmap_node.h>
 #include <rmd/se3.cuh>
 
-rmd::DepthmapNode::DepthmapNode()
+rmd::DepthmapNode::DepthmapNode(ros::NodeHandle &nh)
+  : nh_(nh)
 {
   state_ = rmd::State::TAKE_REFERENCE_FRAME;
+  image_transport::ImageTransport it(nh_);
+  depthmap_publisher_= it.advertise("depth", 10);
 }
 
 bool rmd::DepthmapNode::init()
@@ -137,12 +140,16 @@ void rmd::DepthmapNode::denseInputCallback(
     if(perc_conv > ref_compl_perc_)
     {
       state_ = rmd::State::TAKE_REFERENCE_FRAME;
-#if 1
+
       cv::Mat curr_depth;
       depthmap_->outputDenoisedDepthmap(curr_depth, 0.5f, 400);
-      cv::imshow("curr_depth", rmd::Depthmap::scaleMat(curr_depth));
-      cv::waitKey(2);
-#endif
+
+      cv_bridge::CvImage cv_image;
+      cv_image.header.stamp = ros::Time::now();
+      cv_image.header.frame_id = "depthmap";
+      cv_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+      cv_image.image = curr_depth;
+      depthmap_publisher_.publish(cv_image.toImageMsg());
     }
     break;
   }

diff --git a/src/main_ros.cpp b/src/main_ros.cpp
@@ -26,7 +26,7 @@ int main(int argc, char **argv)
 
   ros::init(argc, argv, "open_remode");
   ros::NodeHandle nh;
-  rmd::DepthmapNode dm_node;
+  rmd::DepthmapNode dm_node(nh);
   if(!dm_node.init())
   {
     ROS_ERROR("could not initialize DepthmapNode. Shutting down node...");