Publish pointcloud messages in ROS

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

web/docker_entrypoint.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+source /opt/ros/melodic/setup.bash
+roscore &
+sleep 60
+npm start
+

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",

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,27 +24,39 @@ 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) {
     cloud->push_back(pcl::PointXYZ(data[i], data[i + 1], data[i + 2]));
   }
 
   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)
   );
   return exports;
 }
 
 NODE_API_MODULE(addon, Init)
+