From 66c3df46521a08c7cf0188c7e14a80999b9b6d79 Mon Sep 17 00:00:00 2001
From: gregklein <greg.klein@tri.global>
Date: Fri, 17 May 2019 15:43:36 -0700
Subject: [PATCH] Publish pointcloud messages in ROS

---
 web/Dockerfile           | 13 ++++++++-----
 web/docker_entrypoint.sh |  6 ++++++
 web/server/binding.gyp   | 11 +++++++++++
 web/server/main.cpp      | 30 ++++++++++++++++++++++++------
 4 files changed, 49 insertions(+), 11 deletions(-)
 create mode 100755 web/docker_entrypoint.sh

diff --git a/web/Dockerfile b/web/Dockerfile
index b58f0a6..c1d499b 100644
--- a/web/Dockerfile
+++ b/web/Dockerfile
@@ -1,12 +1,11 @@
-FROM ubuntu:18.04
+FROM ros:latest
 
 RUN apt-get update
-RUN apt-get upgrade -y
 
 ENV DEBIAN_FRONTEND=noninteractive
 
-RUN apt-get install -y wget cmake build-essential vim libeigen3-dev libflann-dev \
-  libpcap0.8-dev libvtk6-dev libboost-all-dev less
+RUN apt-get upgrade -y && apt-get install -y wget cmake build-essential vim libeigen3-dev libflann-dev \
+  libpcap0.8-dev libvtk6-dev libboost-all-dev ros-melodic-pcl-conversions less && rm -rf /var/lib/apt/lists/*
 
 RUN wget -qO - https://deb.nodesource.com/setup_11.x | bash - \
  && apt-get -y install nodejs
@@ -49,4 +48,8 @@ WORKDIR /usr/src/server
 RUN npm run compile
 EXPOSE 3002
 
-CMD [ "npm", "start" ]
+RUN source /opt/ros/melodic/setup.bash && roscore >> /tmp/roscorelog &
+COPY docker_entrypoint.sh .
+
+#CMD [ "npm", "start" ]
+CMD ./docker_entrypoint.sh
\ No newline at end of file
diff --git a/web/docker_entrypoint.sh b/web/docker_entrypoint.sh
new file mode 100755
index 0000000..95c4e57
--- /dev/null
+++ b/web/docker_entrypoint.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+source /opt/ros/melodic/setup.bash
+roscore &
+sleep 60
+npm start
+
diff --git a/web/server/binding.gyp b/web/server/binding.gyp
index 2fab8fb..a94f56a 100644
--- a/web/server/binding.gyp
+++ b/web/server/binding.gyp
@@ -18,10 +18,21 @@
     "include_dirs": [
       "/usr/include/eigen3",
       "/usr/local/include/pcl-1.9",
+      "/opt/ros/melodic/include",
       "<!@(node -p \"require('node-addon-api').include\")"
     ],
     "libraries": [
       "-Wl,-rpath,/usr/local/lib",
+      "-Wl,-rpath,/opt/ros/melodic/lib",
+      "-L/opt/ros/melodic/lib",
+      "-lroscpp",
+      "-lrosconsole",
+      "-lrostime",
+      "-lroscpp_serialization",
+      "-lboost_system",
+      "-lboost_thread",
+      "-pthread",
+      "-lactionlib",
       "-lpcl_common",
       "-lpcl_features",
       "-lpcl_filters",
diff --git a/web/server/main.cpp b/web/server/main.cpp
index e0c8d0d..aa4ad12 100644
--- a/web/server/main.cpp
+++ b/web/server/main.cpp
@@ -1,13 +1,19 @@
 #include <iostream>
 #include <napi.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/conversions.h>
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <unordered_map>
 
-std::unordered_map<std::string, int> pointcloud_counts;
-std::unordered_map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
 
+#include "ros/ros.h"
+#include <pcl_conversions/pcl_conversions.h>
+#include "sensor_msgs/PointCloud2.h"
+
+std::unordered_map<std::string, ros::Publisher> pointcloud_publishers;
+std::unordered_map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
+std::shared_ptr<ros::NodeHandle> nodehandle;
 
 void processPointCloud(const Napi::CallbackInfo& info )
 {
@@ -18,11 +24,12 @@ void processPointCloud(const Napi::CallbackInfo& info )
   std::string lidarname = static_cast<std::string>(lidarId);
   float* data = originalData.Data();
 
-  const auto& itr = pointcloud_counts.find(lidarname);
-  if (itr == pointcloud_counts.end()) {
-      pointcloud_counts.emplace(lidarname, 0);
+  const auto& itr = pointcloud_publishers.find(lidarname);
+  if (itr == pointcloud_publishers.end()) {
+      pointcloud_publishers.emplace(lidarname,
+        nodehandle->advertise<sensor_msgs::PointCloud2>(lidarname, 3));
   }
-  pointcloud_counts[lidarname]++;
+
 
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
   for (unsigned int i = 0; i < originalData.Length(); i += 3) {
@@ -30,11 +37,21 @@ void processPointCloud(const Napi::CallbackInfo& info )
   }
 
   clouds.emplace(lidarname, cloud);
+  sensor_msgs::PointCloud2 s_cloud;
+  pcl::toROSMsg(*cloud, s_cloud);
+
+  pointcloud_publishers[lidarname].publish(s_cloud);
 
   pcl::io::savePCDFileASCII("tempcloud.pcd", *cloud);
 }
 
 Napi::Object Init(Napi::Env env, Napi::Object exports) {
+  const ros::M_string topic_remappings;
+
+  ros::init(topic_remappings, "pointcloud_cpp");
+
+  nodehandle = std::make_shared<ros::NodeHandle>();
+
   exports.Set(
     "processPointCloud", Napi::Function::New(env, processPointCloud)
   );
@@ -42,3 +59,4 @@ Napi::Object Init(Napi::Env env, Napi::Object exports) {
 }
 
 NODE_API_MODULE(addon, Init)
+