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; +}