back port of Point Cloud support

CMakeLists.txt

@@ -14,6 +14,12 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/cmake/Modules/)
 
 find_package(Jsoncpp REQUIRED)
 
+# Find PCL dependencies
+find_package(PCL REQUIRED COMPONENTS common io)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
 #set the default path for built executables to the "bin" directory
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
@@ -31,6 +37,7 @@ set(IRsourceFiles ${PROJECT_SOURCE_DIR}/src/startIR.cpp)
 set(DEPTHsourceFiles ${PROJECT_SOURCE_DIR}/src/startDepth.cpp)
 set(AUDIOsourceFiles ${PROJECT_SOURCE_DIR}/src/startAudio.cpp)
 set(BODYsourceFiles ${PROJECT_SOURCE_DIR}/src/startBody.cpp)
+set(POINTCLOUDsourceFiles ${PROJECT_SOURCE_DIR}/src/startPointCloud.cpp)
 
 include_directories(${includeFolders})
 include_directories(${JSONCPP_INCLUDE_DIR})
@@ -40,6 +47,8 @@ rosbuild_add_executable(startIR ${IRsourceFiles})
 rosbuild_add_executable(startDepth ${DEPTHsourceFiles})
 rosbuild_add_executable(startAudio ${AUDIOsourceFiles})
 rosbuild_add_executable(startBody ${BODYsourceFiles})
+rosbuild_add_executable(startPointCloud ${POINTCLOUDsourceFiles})
 
 target_link_libraries(startAudio ${JSONCPP_LIBRARY})
 target_link_libraries(startBody ${JSONCPP_LIBRARY})
+target_link_libraries(startPointCloud ${PCL_LIBRARIES})

launch/k2_client.launch

@@ -10,5 +10,6 @@
         <node name="startIR" pkg="k2_client" type="startIR"/>
         <node name="startBody" pkg="k2_client" type="startBody"/>
         <node name="startAudio" pkg="k2_client" type="startAudio"/>
+        <node name="startPointCloud" pkg="k2_client" type="startPointCloud"/>
     </group>
 </launch>

manifest.xml

@@ -14,6 +14,7 @@
   <depend package="cv_bridge"/>
   <depend package="image_transport"/>
   <depend package="camera_info_manager"/>
+  <depend package="pcl_ros"/>
 
 </package>
 

src/startPointCloud.cpp

@@ -0,0 +1,52 @@
+#include "k2_client.h"
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+using namespace std;
+
+typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
+
+int pointBufferSize = 2605056;
+int numberOfPoints = pointBufferSize / sizeof(float); // Hacky, much?
+int streamSize = pointBufferSize + sizeof(double);
+
+int main(int argC,char **argV)
+{
+	ros::init(argC,argV,"startPointCloud");
+	ros::NodeHandle n;
+
+	std::string serverAddress;
+	n.getParam("/serverNameOrIP",serverAddress);
+	Socket mySocket(serverAddress.c_str(),"9005",streamSize);
+
+	ros::Publisher pub = n.advertise<PointCloud>("point_cloud",1);
+
+	PointCloud::Ptr pc (new PointCloud);
+
+	pc->header.frame_id =  ros::this_node::getNamespace().substr(1,std::string::npos) + "/kinect_pcl";
+	while(ros::ok())
+	{
+		// TODO(Somhtr): change to ROS' logging API
+		cout << "Reading data..." << endl;
+		mySocket.readData();
+
+		// TODO(Somhtr): change to ROS' logging API
+		cout << "Copying data..." << endl;
+		float* pt_coords = reinterpret_cast<float*>(mySocket.mBuffer);
+		for(uint64_t idx=0; idx<numberOfPoints; idx+=3)
+		{
+			pc->push_back(pcl::PointXYZ(
+				pt_coords[idx], pt_coords[idx+1], pt_coords[idx+2]
+			));
+		}
+
+		double utcTime;
+		memcpy(&utcTime,&mySocket.mBuffer[pointBufferSize],sizeof(double));
+		pc->header.stamp = ros::Time(utcTime).toSec();
+
+		pub.publish(pc);
+		pc->clear();
+
+		ros::spinOnce();
+	}
+	return 0;
+}