From a40d3cf20d739637f889dda8d6e8b0fc3cc612db Mon Sep 17 00:00:00 2001
From: Rich Wareham <rjw57@cam.ac.uk>
Date: Thu, 5 Mar 2015 15:15:48 +0000
Subject: [PATCH] back port of Point Cloud support

A port of our support for publishing point cloud messages from the
k2_server PR at [1]. This is a port from our internal Catkinized
version of k2_client. Since we don't use rosbuild in our setup this port
is not very well tested compared to our Catkin one. In particular I'm
unsure if rosbuild handles finding PCL in the same way.

If there's interest in accepting a "Catkinization" patch first, we can
put one together.

https://github.com/personalrobotics/k2_server/pull/1
---
 CMakeLists.txt          |  9 +++++++
 launch/k2_client.launch |  1 +
 manifest.xml            |  1 +
 src/startPointCloud.cpp | 52 +++++++++++++++++++++++++++++++++++++++++
 4 files changed, 63 insertions(+)
 create mode 100644 src/startPointCloud.cpp

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 @@
         <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>
diff --git a/manifest.xml b/manifest.xml
index d982405..a692cd9 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -14,6 +14,7 @@
   <depend package="cv_bridge"/>
   <depend package="image_transport"/>
   <depend package="camera_info_manager"/>
+  <depend package="pcl_ros"/>
 
 </package>
 
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 <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;
+}