diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1969bb9..db8aa76 100644
--- a/CMakeLists.txt
+++ b/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})
diff --git a/launch/k2_client.launch b/launch/k2_client.launch
index d6b20e9..151344f 100644
--- a/launch/k2_client.launch
+++ b/launch/k2_client.launch
@@ -10,5 +10,6 @@
+
diff --git a/manifest.xml b/manifest.xml
index d982405..a692cd9 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -14,6 +14,7 @@
+
diff --git a/src/startPointCloud.cpp b/src/startPointCloud.cpp
new file mode 100644
index 0000000..aad3ef0
--- /dev/null
+++ b/src/startPointCloud.cpp
@@ -0,0 +1,52 @@
+#include "k2_client.h"
+#include
+#include
+using namespace std;
+
+typedef pcl::PointCloud 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("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(mySocket.mBuffer);
+ for(uint64_t idx=0; idxpush_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;
+}