New point_cloud2_assembler for PointCloud2 input

Author: jspricke

CMakeLists.txt

@@ -17,6 +17,8 @@ rosbuild_add_executable(laser_scan_assembler src/laser_scan_assembler.cpp)
 rosbuild_link_boost(laser_scan_assembler system signals)
 rosbuild_add_executable(point_cloud_assembler src/point_cloud_assembler.cpp)
 rosbuild_link_boost(point_cloud_assembler signals)
+rosbuild_add_executable(point_cloud2_assembler src/point_cloud2_assembler.cpp)
+rosbuild_link_boost(point_cloud2_assembler signals)
 
 rosbuild_add_executable(../test/dummy_scan_producer test/dummy_scan_producer.cpp)
 rosbuild_link_boost(../test/dummy_scan_producer thread signals)

src/point_cloud2_assembler.cpp

@@ -0,0 +1,92 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  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 the Willow Garage 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 ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS 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 THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+#include "laser_assembler/base_assembler.h"
+#include <sensor_msgs/point_cloud_conversion.h>
+
+using namespace std ;
+
+namespace laser_assembler
+{
+
+/**
+ * \brief Maintains a history of incremental point clouds (usually from laser scans) and generates a point cloud upon request
+ * \todo Clean up the doxygen part of this header
+ * params
+ *  * (Several params are inherited from BaseAssemblerSrv)
+ */
+class PointCloud2Assembler : public BaseAssembler<sensor_msgs::PointCloud2>
+{
+public:
+  PointCloud2Assembler() : BaseAssembler<sensor_msgs::PointCloud2>("max_clouds")
+  {
+
+  }
+
+  ~PointCloud2Assembler()
+  {
+
+  }
+
+  unsigned int GetPointsInScan(const sensor_msgs::PointCloud2& scan)
+  {
+    return (scan.width * scan.height);
+  }
+
+  void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::PointCloud2& scan_in, sensor_msgs::PointCloud& cloud_out)
+  {
+    sensor_msgs::PointCloud cloud_in;
+    sensor_msgs::convertPointCloud2ToPointCloud(scan_in, cloud_in);
+    tf_->transformPointCloud(fixed_frame_id, cloud_in, cloud_out) ;
+    return ;
+  }
+
+private:
+
+};
+
+}
+
+using namespace laser_assembler ;
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "point_cloud_assembler");
+  PointCloud2Assembler pc_assembler;
+  pc_assembler.start("cloud");
+  ros::spin();
+
+  return 0;
+}