Mapping server transport dynamic cloud downsampling

docker/cloudsim_bridge/Dockerfile

@@ -79,11 +79,14 @@ VOLUME /root/.aws
 RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \
  && sudo apt-get update \
  && sudo apt-get install -y \
+    libpcl-dev \
+    libpcl-conversions-dev \
     python-catkin-tools \
     python-rosdep \
     python-rosinstall \
     ros-melodic-desktop \
     ros-melodic-joystick-drivers \
+    ros-melodic-pcl-ros \
     ros-melodic-pointcloud-to-laserscan \
     ros-melodic-robot-localization \
     ros-melodic-spacenav-node \

docker/cloudsim_sim/Dockerfile

@@ -51,11 +51,14 @@ VOLUME /root/.aws
 RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \
  && sudo apt-get update -qq \
  && sudo apt-get install -y -qq \
+    libpcl-dev \
+    libpcl-conversions-dev \
     python-catkin-tools \
     python-rosdep \
     python-rosinstall \
     ros-melodic-desktop \
     ros-melodic-joystick-drivers \
+    ros-melodic-pcl-ros \
     ros-melodic-pointcloud-to-laserscan \
     ros-melodic-robot-localization \
     ros-melodic-spacenav-node \

docker/subt_shell/Dockerfile

@@ -67,11 +67,14 @@ RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu
  && sudo /bin/sh -c 'apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' \
  && sudo apt-get update -qq \
  && sudo apt-get install -y -qq \
+    libpcl-dev \
+    libpcl-conversions-dev \
     python-catkin-tools \
     python-rosdep \
     python-rosinstall \
     ros-melodic-desktop \
     ros-melodic-joystick-drivers \
+    ros-melodic-pcl-ros \
     ros-melodic-pointcloud-to-laserscan \
     ros-melodic-robot-localization \
     ros-melodic-spacenav-node \

docker/subt_sim_entry/Dockerfile

@@ -49,11 +49,14 @@ VOLUME /root/.aws
 RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \
  && sudo apt-get update -qq \
  && sudo apt-get install -y -qq \
+    libpcl-dev \
+    libpcl-conversions-dev \
     python-catkin-tools \
     python-rosdep \
     python-rosinstall \
     ros-melodic-desktop \
     ros-melodic-joystick-drivers \
+    ros-melodic-pcl-ros \
     ros-melodic-pointcloud-to-laserscan \
     ros-melodic-robot-localization \
     ros-melodic-spacenav-node \

docker/subt_team_entry/Dockerfile

@@ -65,11 +65,14 @@ RUN export DEBIAN_FRONTEND=noninteractive \
 RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \
  && sudo apt-get update -qq \
  && sudo apt-get install -y -qq \
+    libpcl-dev \
+    libpcl-conversions-dev \
     python-catkin-tools \
     python-rosdep \
     python-rosinstall \
     ros-melodic-desktop \
     ros-melodic-joystick-drivers \
+    ros-melodic-pcl-ros \
     ros-melodic-pointcloud-to-laserscan \
     ros-melodic-robot-localization \
     ros-melodic-spacenav-node \

subt_ros/CMakeLists.txt

@@ -40,6 +40,7 @@ find_package(ignition-common3 REQUIRED)
 find_package(ignition-math6 REQUIRED)
 find_package(ignition-msgs6 REQUIRED)
 find_package(ignition-transport9 REQUIRED)
+find_package(PCL REQUIRED)
 
 catkin_package(
   CATKIN_DEPENDS
@@ -51,6 +52,7 @@ catkin_package(
 
 include_directories(
   ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
 )
 
 ## Pose->TF broadcaster
@@ -62,6 +64,16 @@ target_link_libraries(pose_tf_broadcaster
   ${catkin_LIBRARIES}
 )
 
+## Cloud Downsample
+add_executable(cloud_downsampler
+  src/cloud_downsample.cpp
+)
+
+target_link_libraries(cloud_downsampler
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
 add_executable(optical_frame_publisher src/OpticalFramePublisher.cc)
 target_link_libraries(optical_frame_publisher
   ${catkin_LIBRARIES}
@@ -114,7 +126,7 @@ target_link_libraries(set_rate_relay
 )
 add_dependencies(set_rate_relay ${catkin_EXPORTED_TARGETS})
 
-install(TARGETS pose_tf_broadcaster subt_ros_relay set_pose_relay bridge_logger
+install(TARGETS pose_tf_broadcaster cloud_downsampler subt_ros_relay set_pose_relay bridge_logger
   optical_frame_publisher set_rate_relay
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

subt_ros/launch/models_common/mapping_server_relays.launch

@@ -1,6 +1,10 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="name"/>
+  <param name="use_sim_time" value="true"/>
+
+  <node pkg="subt_ros" type="cloud_downsampler" name="cloud_downsampler" output="screen"/>
+
   <node
     pkg="topic_tools"
     type="throttle"
@@ -12,7 +16,14 @@
     type="parameter_bridge"
     name="ros_ign_bridge_mapping_server_cloud"
     args="/model/$(arg name)/cloud@sensor_msgs/PointCloud2]ignition.msgs.PointCloudPacked">
-    <remap from="/model/$(arg name)/cloud" to="/cloud_throttled"/>
+    <remap from="/model/$(arg name)/cloud" to="/cloud_throttled_downsampled"/>
+  </node>
+  <node
+    pkg="ros_ign_bridge"
+    type="parameter_bridge"
+    name="ros_ign_bridge_mapping_server_cloud_size"
+    args="/model/$(arg name)/cloud_size@std_msgs/Int32]ignition.msgs.Int32">
+    <remap from="/model/$(arg name)/cloud_size" to="/cloud_throttled_size"/>
   </node>
   <node
     pkg="topic_tools"

subt_ros/src/cloud_downsample.cpp

@@ -0,0 +1,111 @@
+#include <ros/ros.h>
+#include <std_msgs/Int32.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl_ros/transforms.h>
+#include <pcl/filters/voxel_grid.h>
+
+class PCDownSampler {
+  protected:
+    ros::NodeHandle nh_;
+    ros::NodeHandle pnh_;
+
+    ros::Subscriber cloud_sub_;
+    ros::Publisher cloud_pub_;
+    ros::Publisher original_size_pub_;
+
+    float leaf_size_;
+
+    pcl::VoxelGrid<pcl::PCLPointCloud2> voxel_filter_;
+
+    void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
+    void publishCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const std_msgs::Header& header);
+
+  public:
+    PCDownSampler();
+};
+
+void PCDownSampler::
+cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
+  // Publish original size
+  std_msgs::Int32 size_msg;
+  size_msg.data = msg->height * msg->width;
+  original_size_pub_.publish(size_msg);
+
+  // Convert cloud to PCL format
+  pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2 ());
+  pcl_conversions::toPCL(*msg, *cloud);
+
+  // Perform initial down sampling
+  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
+  voxel_filter_.setInputCloud(cloud);
+  voxel_filter_.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
+  voxel_filter_.filter(*cloud_filtered);
+
+  // Determine if additional down sampling is required
+  while(cloud_filtered->width * cloud_filtered->height > 600000)
+  {
+    // Increase new leaf size and down sample again until the cloud size is below the initial cloud size
+    leaf_size_ = leaf_size_ + 0.02f;
+    voxel_filter_.setInputCloud(cloud);
+    voxel_filter_.setLeafSize(leaf_size_, leaf_size_, leaf_size_);
+    voxel_filter_.filter(*cloud_filtered);
+  }
+
+  // Publish pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromPCLPointCloud2(*cloud_filtered, *(tmp));
+  publishCloud(tmp, msg->header);
+}
+
+void PCDownSampler::
+publishCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const std_msgs::Header& header) {
+  sensor_msgs::PointCloud2 msg;
+  msg.data.resize(12 * cloud->points.size());
+  msg.width = cloud->points.size();
+  msg.height = 1;
+  msg.row_step = cloud->points.size() * 12;
+  msg.is_dense = true;
+  sensor_msgs::PointField fx;
+  fx.name="x";
+  fx.offset = 0;
+  fx.datatype = 7;
+  fx.count = 1;
+  msg.fields.push_back(fx);
+  fx.name="y";
+  fx.offset = 4;
+  msg.fields.push_back(fx);
+  fx.name="z";
+  fx.offset=8;
+  msg.fields.push_back(fx);
+  msg.point_step = 12;
+  for (size_t i = 0; i < cloud->points.size(); i++) {
+    *(float*)(&msg.data[i*12 + 0]) = cloud->points[i].x;
+    *(float*)(&msg.data[i*12 + 4]) = cloud->points[i].y;
+    *(float*)(&msg.data[i*12 + 8]) = cloud->points[i].z;
+  }
+
+  msg.header.seq = header.seq;
+  msg.header.stamp = header.stamp;
+  msg.header.frame_id = "artifact_origin";
+  cloud_pub_.publish(msg);
+}
+
+PCDownSampler::PCDownSampler() {
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_("~");
+  leaf_size_ = 0.1f;
+  cloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("/cloud_throttled", 1, &PCDownSampler::cloudCallback, this);
+  cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/cloud_throttled_downsampled", 1, true);
+  original_size_pub_ = nh_.advertise<std_msgs::Int32>("/cloud_throttled_size", 1, true);
+}
+
+//////////////////////////////////////////////////
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "cloud_downsample");
+  PCDownSampler pc_down_sampler;
+  ros::spin();
+  return 0;
+}