RGB/depth point-cloud conversion fixed

but-spanel · May 1, 2012 · 17c8467 · 17c8467
1 parent 8866f66
commit 17c8467
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 9 deletions.
diff --git a/srs_assisted_arm_navigation/launch/planning_env_but.launch b/srs_assisted_arm_navigation/launch/planning_env_but.launch
@@ -22,6 +22,8 @@
     <node pkg="robot_self_filter" type="self_filter" respawn="true" name="stereo_self_filter" output="screen">
       <remap from="cloud_in" to="/cam3d/depth/points" />
       <remap from="cloud_out" to="/cam3d/depth/points_filtered2" />
+<!--      <remap from="cloud_in" to="/cam3d/rgb/points" />
+      <remap from="cloud_out" to="/cam3d/rgb/points_filtered2" />-->
       <param name="sensor_frame" type="string" value="head_cam3d_link" />
       <param name="subsample_value" type="double" value=".02"/>
       <rosparam command="load" file="$(find cob_arm_navigation)/config/$(env ROBOT)/self_filter.yaml" />
@@ -39,6 +41,8 @@
   <!-- Run BUT dynamic scene model server node -->
   <node name="but_server_node" pkg="srs_env_model" type="but_server_node">
     <param name="pointcloud_subscriber" value="/cam3d/depth/points_filtered2"/>
+    <param name="input_has_rgb" type="bool" value="false"/>
+    <!--param name="pointcloud_subscriber" value="/cam3d/rgb/points_filtered2"/-->
     <param name="max_range" type="double" value="2.5"/>
     <param name="pointcloud_min_z" value="0.2" type="double"/>
     <param name="resolution" value="0.025" type="double"/>
@@ -48,3 +52,4 @@
 
 
 </launch>
+
diff --git a/srs_env_model/include/but_server/plugins/PointCloudPlugin.h b/srs_env_model/include/but_server/plugins/PointCloudPlugin.h
@@ -91,6 +91,10 @@ namespace srs
         */
         void insertCloudCallback(const tIncommingPointCloud::ConstPtr& cloud);
 
+        /**
+         * Test if incomming pointcloud2 has rgb part
+         */
+        bool isRGBCloud( const tIncommingPointCloud::ConstPtr& cloud );
 
     protected:
         //! Is publishing enabled?
@@ -139,6 +143,9 @@ namespace srs
         //! Counter
         long counter;
 
+        //! Has input cloud rgb data?
+        bool m_bUseRGB;
+
     }; // class CPointCloudPlugin
 
     /// Declare holder object - partial specialization of the default holder with predefined connection settings

diff --git a/srs_env_model/src/but_server/plugins/PointCloudPlugin.cpp b/srs_env_model/src/but_server/plugins/PointCloudPlugin.cpp
@@ -36,7 +36,6 @@
 
 #define POINTCLOUD_CENTERS_PUBLISHER_NAME std::string("butsrv_pointcloud_centers")
 #define SUBSCRIBER_POINT_CLOUD_NAME std::string("/cam3d/rgb/points")
-//#define SUBSCRIBER_POINT_CLOUD_NAME std::string("/cam3d/depth/points_filtered2")
 #define DEFAULT_FRAME_ID std::string("/head_cam3d_link")
 #define BASE_FRAME_ID std::string("base_footprint")
 
@@ -52,6 +51,7 @@ srs::CPointCloudPlugin::CPointCloudPlugin(const std::string & name, bool subscri
 , m_bFilterPC(true)
 , m_pointcloudMinZ(-std::numeric_limits<double>::max())
 , m_pointcloudMaxZ(std::numeric_limits<double>::max())
+, m_bUseRGB( false )
 {
 	m_data = new tData;
 	assert( m_data != 0 );
@@ -86,6 +86,9 @@ void srs::CPointCloudPlugin::init(ros::NodeHandle & node_handle)
 	node_handle.param("pointcloud_min_z", m_pointcloudMinZ, m_pointcloudMinZ);
 	node_handle.param("pointcloud_max_z", m_pointcloudMaxZ, m_pointcloudMaxZ);
 
+	// Contains input pointcloud RGB?
+	node_handle.param("input_has_rgb", m_bUseRGB, m_bUseRGB );
+
 	// Create publisher
 	m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 100, m_latchedTopics);
 
@@ -228,19 +231,23 @@ void srs::CPointCloudPlugin::insertCloudCallback( const  tIncommingPointCloud::C
 
 	// Convert input pointcloud
 	m_data->clear();
-//	pcl::fromROSMsg(*cloud, *m_data);
-
+	if( ! isRGBCloud( cloud ) )
+	{
+		pcl::PointCloud< pcl::PointXYZ >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZ> );
 
-	/*
-	pcl::fromROSMsg(*cloud, *m_data);
+		pcl::fromROSMsg(*cloud, *bufferCloud );
+		pcl::copyPointCloud< pcl::PointXYZ, tPclPoint >( *bufferCloud, *m_data );
 
-/*/
-	pcl::PointCloud< pcl::PointXYZRGB >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZRGB > );
+	}
+	else
+	{
+		pcl::PointCloud< pcl::PointXYZRGB >::Ptr bufferCloud( new pcl::PointCloud< pcl::PointXYZRGB > );
 
-	pcl::fromROSMsg(*cloud, *bufferCloud);
+		pcl::fromROSMsg(*cloud, *bufferCloud);
+		pcl::copyPointCloud<pcl::PointXYZRGB, tPclPoint>( *bufferCloud, *m_data );
+	}
 
 
-	pcl::copyPointCloud<pcl::PointXYZRGB, tPclPoint>( *bufferCloud, *m_data );
 	//*/
 
 	// If different frame id
@@ -334,3 +341,11 @@ bool srs::CPointCloudPlugin::shouldPublish()
 	return( m_publishPointCloud && m_pcPublisher.getNumSubscribers() > 0 );
 }
 
+/**
+ * Test if incomming pointcloud2 has rgb part - parameter driven
+ */
+bool srs::CPointCloudPlugin::isRGBCloud( const tIncommingPointCloud::ConstPtr& cloud )
+{
+	return m_bUseRGB;
+}
+