Addapt the pointlcoud_to_webgl script to the new format of the website

plnegre · plnegre · commit 0a1671f508dc · 2014-10-28T17:11:21.000+01:00
diff --git a/bag_tools/src/extract_images.cpp b/bag_tools/src/extract_images.cpp
@@ -2,28 +2,28 @@
 /// Systems, Robotics and Vision Group
 /// University of the Balearican Islands
 /// All rights reserved.
-/// 
+///
 /// Redistribution and use in source and binary forms, with or without
 /// modification, are permitted provided that the following conditions are met:
 ///     * Redistributions of source code must retain the above copyright
 ///       notice, this list of conditions and the following disclaimer.
 ///     * Redistributions in binary form must reproduce the above copyright
 ///       notice, this list of conditions and the following disclaimer in the
 ///       documentation and/or other materials provided with the distribution.
-///     * Neither the name of Systems, Robotics and Vision Group, University of 
-///       the Balearican Islands nor the names of its contributors may be used 
-///       to endorse or promote products derived from this software without 
+///     * Neither the name of Systems, Robotics and Vision Group, University of
+///       the Balearican Islands nor the names of its contributors may be used
+///       to endorse or promote products derived from this software without
 ///       specific prior written permission.
-/// 
-/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
-/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
-/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+///
+/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 /// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-/// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 
+/// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <ros/ros.h>
@@ -47,7 +47,7 @@
 class ImageSaver
 {
 public:
-  ImageSaver(const std::string& save_dir, const std::string& filetype, 
+  ImageSaver(const std::string& save_dir, const std::string& filetype,
       const std::string& prefix) :
     save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0)
   {}
@@ -108,7 +108,7 @@ int main(int argc, char** argv)
   std::string out_dir(argv[1]);
   std::string filetype(argv[2]);
   std::string image_topic(argv[3]);
-  
+
   ros::Time::init();
 
   std::string prefix("image");
diff --git a/pointcloud_tools/src/pointcloud_mapper.cpp b/pointcloud_tools/src/pointcloud_mapper.cpp
@@ -32,8 +32,7 @@ class PointCloudMapper
     nh_priv_.param("filter_map", filter_map_, false);
 
     cloud_sub_ = nh_.subscribe<PointCloud>("input", 1, &PointCloudMapper::callback, this);
-    bool latched = true;
-    cloud_pub_ = nh_priv_.advertise<PointCloud>("output", 1, latched);
+    cloud_pub_ = nh_priv_.advertise<PointCloud>("output", 1, true);
     pub_timer_ = nh_.createWallTimer(ros::WallDuration(3.0), &PointCloudMapper::publishCallback, this);
   }
 
@@ -42,8 +41,8 @@ class PointCloudMapper
     ROS_INFO_STREAM("Received cloud with " << cloud->points.size() << " points.");
     PointCloud transformed_cloud;
     ros::Time points_time;
-    points_time.fromNSec((*cloud).header.stamp);
-    tf_listener_.waitForTransform(fixed_frame_, (*cloud).header.frame_id, points_time, ros::Duration(5.0));
+    points_time.fromNSec(cloud->header.stamp);
+    tf_listener_.waitForTransform(fixed_frame_, cloud->header.frame_id, points_time, ros::Duration(5.0));
     bool success = pcl_ros::transformPointCloud(fixed_frame_, *cloud, transformed_cloud, tf_listener_);
     if (success)
     {
@@ -55,6 +54,8 @@ class PointCloudMapper
         accumulated_cloud_ = *cloud_downsampled;
       }
 
+      ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points.");
+
       // Publish the cloud
       if (cloud_pub_.getNumSubscribers() > 0)
         cloud_pub_.publish(accumulated_cloud_);
@@ -64,8 +65,6 @@ class PointCloudMapper
     {
       ROS_ERROR("Could not transform point cloud to %s", fixed_frame_.c_str());
     }
-
-    ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points.");
   }
 
   void publishCallback(const ros::WallTimerEvent&)
diff --git a/pointcloud_tools/src/pointcloud_to_webgl.cpp b/pointcloud_tools/src/pointcloud_to_webgl.cpp
@@ -1,175 +1,115 @@
 #include <ros/ros.h>
 
-#include <std_msgs/String.h>
+#include <boost/lexical_cast.hpp>
 
 #include <pcl/point_types.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/filters/voxel_grid.h>
 #include <pcl/common/centroid.h>
 
 class PointCloudToWebgl {
 
-// ROS properties
-ros::NodeHandle nh_;
-ros::NodeHandle nh_private_;
-
-std::string pcd_filename_;
-double max_ascii_file_size_;
-int pcd_type_;
-
 public:
 
   /**
    * Class constructor
    */
-  PointCloudToWebgl() : nh_private_("~")
+  PointCloudToWebgl(const std::string& input_cloud, const int& cloud_format,
+      const std::string& output_cloud) :
+    input_cloud_(input_cloud), cloud_format_(cloud_format), output_cloud_(output_cloud)
+  {}
+
+  /**
+   * Convert the cloud
+   */
+  void convert()
   {
-    // Read the parameters from the parameter server (set defaults)
-    nh_private_.param("pcd_filename", pcd_filename_, std::string("pointcloud_file.pcd"));
-    nh_private_.param("max_ascii_file_size", max_ascii_file_size_, 4.0);  // In MBytes
-    nh_private_.param("pcd_type", pcd_type_, 0);  // 0 -> XYZ | 1 -> XYZRGB
+    ROS_INFO_STREAM("[PointCloudToWebgl:] Opening file " << input_cloud_);
 
-    ROS_INFO_STREAM("[PointCloudToWebgl:] Opening file " << pcd_filename_);
+    // Init the cloud
+    pcl::PointCloud<pcl::PointXYZRGB> cloud;
 
     // Read point cloud
-    if (pcd_type_ == 0)
+    if (cloud_format_ == 0)
     {
       // NO RGB
-      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
-      if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_filename_, *cloud_ptr) == -1) //* load the file
-      {
-        ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << pcd_filename_);
-      }
-      else
+      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
+      if (pcl::io::loadPCDFile<pcl::PointXYZ> (input_cloud_, *cloud_xyz) == -1) //* load the file
       {
-        // Convert the cloud
-        pcl::PointCloud<pcl::PointXYZ> cloud = *cloud_ptr;
-        int file_point_size = 27;
-        int max_bytes = (int)round(max_ascii_file_size_ * 1024 * 1024);
-        int desired_points = max_bytes / file_point_size;
-        double voxel_size = 0.001;
-        double offset = 0.0002;
-        while ((int)cloud.size() > desired_points)
-        {
-          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled = filter(cloud.makeShared(), voxel_size);
-          cloud = *cloud_downsampled;
-          voxel_size = voxel_size + offset;
-        }
-
-        // Compute the cloud centroid
-        Eigen::Vector4f centroid; 
-        pcl::compute3DCentroid(cloud, centroid);
-
-        // Save int file
-        int lastindex = pcd_filename_.find_last_of("."); 
-        std::string filename = pcd_filename_.substr(0, lastindex); 
-        filename = filename + ".txt";
-        ROS_INFO_STREAM("[PointCloudToWebgl:] Saving webgl file to " << filename);
-        std::fstream f_webgl(filename.c_str(), std::ios::out);
-        for (unsigned int i=0; i<cloud.size(); i++)
-        {
-          f_webgl << cloud[i].x - centroid[0] << "," << 
-                     cloud[i].y - centroid[1] << "," << 
-                     cloud[i].z - centroid[2] << "," << 
-                     (int)(224) << "," << 
-                     (int)(224) << "," << 
-                     (int)(224) << std::endl;
-        }
-        f_webgl.close();
-        ROS_INFO("[PointCloudToWebgl:] Saved!");
+        ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << input_cloud_);
+        return;
       }
+      copyPointCloud(*cloud_xyz, cloud);
     }
-    else
+    else if (cloud_format_ == 1)
     {
       // RGB
-      pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
-      if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (pcd_filename_, *cloud_ptr) == -1) //* load the file
-      {
-        ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << pcd_filename_);
-      }
-      else
+      pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb (new pcl::PointCloud<pcl::PointXYZRGB>);
+      if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (input_cloud_, *cloud_xyzrgb) == -1) //* load the file
       {
-        // Convert the cloud
-        pcl::PointCloud<pcl::PointXYZRGB> cloud = *cloud_ptr;
-        int file_point_size = 37;
-        int max_bytes = (int)round(max_ascii_file_size_ * 1024 * 1024);
-        int desired_points = max_bytes / file_point_size;
-        double voxel_size = 0.001;
-        double offset = 0.0002;
-        while ((int)cloud.size() > desired_points)
-        {
-          pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_downsampled = filter(cloud.makeShared(), voxel_size);
-          cloud = *cloud_downsampled;
-          voxel_size = voxel_size + offset;
-        }
-
-        // Compute the cloud centroid
-        Eigen::Vector4f centroid; 
-        pcl::compute3DCentroid(cloud, centroid);
-
-        // Save int file
-        int lastindex = pcd_filename_.find_last_of("."); 
-        std::string filename = pcd_filename_.substr(0, lastindex); 
-        filename = filename + ".txt";
-        ROS_INFO_STREAM("[PointCloudToWebgl:] Saving webgl file to " << filename);
-        std::fstream f_webgl(filename.c_str(), std::ios::out);
-        for (unsigned int i=0; i<cloud.size(); i++)
-        {
-          f_webgl << cloud[i].x - centroid[0] << "," << 
-                     cloud[i].y - centroid[1] << "," << 
-                     cloud[i].z - centroid[2] << "," << 
-                     (int)cloud[i].r << "," << 
-                     (int)cloud[i].g << "," << 
-                     (int)cloud[i].b << std::endl;
-        }
-        f_webgl.close();
-        ROS_INFO("[PointCloudToWebgl:] Saved!");
+        ROS_ERROR_STREAM("[PointCloudToWebgl:] Couldn't read file " << input_cloud_);
+        return;
       }
+      copyPointCloud(*cloud_xyzrgb, cloud);
     }
-  }
 
-  pcl::PointCloud<pcl::PointXYZ>::Ptr filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double voxel_size)
-  {
-    // Downsampling using voxel grid
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::VoxelGrid<pcl::PointXYZ> grid_;
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled_ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    grid_.setLeafSize(voxel_size,
-                      voxel_size,
-                      voxel_size);
-    grid_.setDownsampleAllData(true);
-    grid_.setInputCloud(cloud);
-    grid_.filter(*cloud_downsampled_ptr);
-    return cloud_downsampled_ptr;
-  }
+    // Compute the cloud centroid
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(cloud, centroid);
 
-  pcl::PointCloud<pcl::PointXYZRGB>::Ptr filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, double voxel_size)
-  {
-    // Downsampling using voxel grid
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
-    pcl::VoxelGrid<pcl::PointXYZRGB> grid_;
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_downsampled_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-    grid_.setLeafSize(voxel_size,
-                      voxel_size,
-                      voxel_size);
-    grid_.setDownsampleAllData(true);
-    grid_.setInputCloud(cloud);
-    grid_.filter(*cloud_downsampled_ptr);
-    return cloud_downsampled_ptr;
+    // Save int file
+    ROS_INFO_STREAM("[PointCloudToWebgl:] Saving webgl file to " << output_cloud_);
+    std::fstream f_webgl(output_cloud_.c_str(), std::ios::out);
+    for (unsigned int i=0; i<cloud.size(); i++)
+    {
+      int r = 224;
+      int g = 224;
+      int b = 224;
+      if (cloud_format_ == 1)
+      {
+        r = (int)cloud[i].r;
+        g = (int)cloud[i].g;
+        b = (int)cloud[i].b;
+      }
+      f_webgl << cloud[i].x - centroid[0] << "," <<
+                 cloud[i].y - centroid[1] << "," <<
+                 cloud[i].z - centroid[2] << "," <<
+                 r << "," <<
+                 g << "," <<
+                 b << std::endl;
+
+    }
+    f_webgl.close();
+    ROS_INFO("[PointCloudToWebgl:] Saved!");
   }
 
+  private:
+  std::string input_cloud_;
+  std::string output_cloud_;
+  int cloud_format_;
 };
 
 /**
  * Main entry point of the code
  */
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "pointcloud_to_webgl");
-  PointCloudToWebgl node;
-  ros::spin();
+  if (argc < 4)
+  {
+    std::cout << "Usage: " << argv[0] << " INPUT_PCD FORMAT OUTPUT_CSV" << std::endl;
+    std::cout << "  Example: " << argv[0] << " input_cloud.pcd 0 output_cloud.csv" << std::endl;
+    return 0;
+  }
+
+  // Read inputs
+  std::string input_cloud(argv[1]);
+  std::string output_cloud(argv[3]);
+  int cloud_format = boost::lexical_cast<int>(argv[2]);
+
+  // Convert
+  PointCloudToWebgl converter(input_cloud, cloud_format, output_cloud);
+  converter.convert();
+
   return 0;
 }
 

Original file line number	Diff line number	Diff line change
`@@ -32,8 +32,7 @@ class PointCloudMapper`
`32`	`32`	`nh_priv_.param("filter_map", filter_map_, false);`
`33`	`33`
`34`	`34`	`cloud_sub_ = nh_.subscribe<PointCloud>("input", 1, &PointCloudMapper::callback, this);`
`35`		`- bool latched = true;`
`36`		`- cloud_pub_ = nh_priv_.advertise<PointCloud>("output", 1, latched);`
	`35`	`+ cloud_pub_ = nh_priv_.advertise<PointCloud>("output", 1, true);`
`37`	`36`	`pub_timer_ = nh_.createWallTimer(ros::WallDuration(3.0), &PointCloudMapper::publishCallback, this);`
`38`	`37`	`}`
`39`	`38`
`@@ -42,8 +41,8 @@ class PointCloudMapper`
`42`	`41`	`ROS_INFO_STREAM("Received cloud with " << cloud->points.size() << " points.");`
`43`	`42`	`PointCloud transformed_cloud;`
`44`	`43`	`ros::Time points_time;`
`45`		`- points_time.fromNSec((*cloud).header.stamp);`
`46`		`- tf_listener_.waitForTransform(fixed_frame_, (*cloud).header.frame_id, points_time, ros::Duration(5.0));`
	`44`	`+ points_time.fromNSec(cloud->header.stamp);`
	`45`	`+ tf_listener_.waitForTransform(fixed_frame_, cloud->header.frame_id, points_time, ros::Duration(5.0));`
`47`	`46`	`bool success = pcl_ros::transformPointCloud(fixed_frame_, *cloud, transformed_cloud, tf_listener_);`
`48`	`47`	`if (success)`
`49`	`48`	`{`
`@@ -55,6 +54,8 @@ class PointCloudMapper`
`55`	`54`	`accumulated_cloud_ = *cloud_downsampled;`
`56`	`55`	`}`
`57`	`56`
	`57`	`+ ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points.");`
	`58`	`+`
`58`	`59`	`// Publish the cloud`
`59`	`60`	`if (cloud_pub_.getNumSubscribers() > 0)`
`60`	`61`	`cloud_pub_.publish(accumulated_cloud_);`
`@@ -64,8 +65,6 @@ class PointCloudMapper`
`64`	`65`	`{`
`65`	`66`	`ROS_ERROR("Could not transform point cloud to %s", fixed_frame_.c_str());`
`66`	`67`	`}`
`67`		`-`
`68`		`- ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points.");`
`69`	`68`	`}`
`70`	`69`
`71`	`70`	`void publishCallback(const ros::WallTimerEvent&)`