├── launch
    └── point_cloud_converter.launch
├── package.xml
├── CMakeLists.txt
└── src
    └── converter.cpp


/launch/point_cloud_converter.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 	<node pkg="point_cloud_converter" name="point_cloud_converter" type="point_cloud_converter_node" >
 3 | 		<remap from="points_in" to="velodyne_pointcloud_points"/>
 4 | 		<remap from="points2_out" to="velodyne_points" />
 5 | 		<!--
 6 | 		<remap from="points2_in" to="velodyne_points"/>
 7 | 		<remap from="points_out" to="velodyne_points" />
 8 | 		-->
 9 | 	</node>
10 | </launch>


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>point_cloud_converter</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The point_cloud_converter package to convert PointCloud to PointCloud2 or PointCloud2 to PointCloud. Basically the node of
 6 |   http://wiki.ros.org/point_cloud_converter by Radu Bogdan Rusu revived.</description>
 7 | 
 8 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 9 |   <!-- Example:  -->
10 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
11 |   <maintainer email="sam.pfeiffer@pal-robotics.com">Sammy Pfeiffer</maintainer>
12 | 
13 | 
14 |   <license>TODO</license>
15 | 
16 |   <buildtool_depend>catkin</buildtool_depend>
17 |   <build_depend>roscpp</build_depend>
18 |   <build_depend>sensor_msgs</build_depend>
19 |   <run_depend>roscpp</run_depend>
20 |   <run_depend>sensor_msgs</run_depend>
21 | 
22 | 
23 |   <!-- The export tag contains other, unspecified, tags -->
24 |   <export>
25 |     <!-- You can specify that this package is a metapackage here: -->
26 |     <!-- <metapackage/> -->
27 | 
28 |     <!-- Other tools can request additional information be placed here -->
29 | 
30 |   </export>
31 | </package>


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(point_cloud_converter)
 3 | 
 4 | ## Find catkin macros and libraries
 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 6 | ## is used, also find other catkin packages
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   roscpp
 9 |   sensor_msgs
10 | )
11 | 
12 | 
13 | 
14 | catkin_package()
15 | 
16 | ###########
17 | ## Build ##
18 | ###########
19 | 
20 | ## Specify additional locations of header files
21 | ## Your package locations should be listed before other locations
22 | # include_directories(include)
23 | include_directories(
24 |   ${catkin_INCLUDE_DIRS}
25 | )
26 | 
27 | ## Declare a cpp library
28 | # add_library(point_cloud_converter
29 | #   src/${PROJECT_NAME}/point_cloud_converter.cpp
30 | # )
31 | 
32 | ## Declare a cpp executable
33 | add_executable(point_cloud_converter_node src/converter.cpp)
34 | 
35 | ## Add cmake target dependencies of the executable/library
36 | ## as an example, message headers may need to be generated before nodes
37 | # add_dependencies(point_cloud_converter_node point_cloud_converter_generate_messages_cpp)
38 | 
39 | ## Specify libraries to link a library or executable target against
40 | target_link_libraries(point_cloud_converter_node
41 |   ${catkin_LIBRARIES}
42 | )
43 | 
44 | #############
45 | ## Install ##
46 | #############
47 | 
48 | # all install targets should use catkin DESTINATION variables
49 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
50 | 
51 | ## Mark executable scripts (Python etc.) for installation
52 | ## in contrast to setup.py, you can choose the destination
53 | # install(PROGRAMS
54 | #   scripts/my_python_script
55 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
56 | # )
57 | 
58 | ## Mark executables and/or libraries for installation
59 | install(TARGETS point_cloud_converter_node
60 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
61 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
62 |   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
63 | )
64 | 
65 | 
66 | foreach(dir launch)
67 |     install(DIRECTORY ${dir}/
68 |         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
69 | endforeach(dir)
70 | 
71 | 
72 | ## Mark cpp header files for installation
73 | # install(DIRECTORY include/${PROJECT_NAME}/
74 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
75 | #   FILES_MATCHING PATTERN "*.h"
76 | #   PATTERN ".svn" EXCLUDE
77 | # )
78 | 
79 | ## Mark other files for installation (e.g. launch and bag files, etc.)
80 | # install(FILES
81 | #   # myfile1
82 | #   # myfile2
83 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
84 | # )
85 | 
86 | #############
87 | ## Testing ##
88 | #############
89 | 
90 | ## Add gtest based cpp test target and link libraries
91 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_point_cloud_converter.cpp)
92 | # if(TARGET ${PROJECT_NAME}-test)
93 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
94 | # endif()
95 | 
96 | ## Add folders to be run by python nosetests
97 | # catkin_add_nosetests(test)
98 | 


--------------------------------------------------------------------------------
/src/converter.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Software License Agreement (BSD License)
  3 |  *
  4 |  *  Copyright (c) 2009, Willow Garage, Inc.
  5 |  *  All rights reserved.
  6 |  *
  7 |  *  Redistribution and use in source and binary forms, with or without
  8 |  *  modification, are permitted provided that the following conditions
  9 |  *  are met:
 10 |  *
 11 |  *   * Redistributions of source code must retain the above copyright
 12 |  *     notice, this list of conditions and the following disclaimer.
 13 |  *   * Redistributions in binary form must reproduce the above
 14 |  *     copyright notice, this list of conditions and the following
 15 |  *     disclaimer in the documentation and/or other materials provided
 16 |  *     with the distribution.
 17 |  *   * Neither the name of Willow Garage, Inc. nor the names of its
 18 |  *     contributors may be used to endorse or promote products derived
 19 |  *     from this software without specific prior written permission.
 20 |  *
 21 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 22 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 23 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 24 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 25 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 26 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 27 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 28 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 29 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 30 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 31 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 32 |  *  POSSIBILITY OF SUCH DAMAGE.
 33 |  *
 34 |  * $Id: converter.cpp 33249 2010-03-11 02:09:24Z rusu $
 35 |  *
 36 |  */
 37 | 
 38 | #include <ros/ros.h>
 39 | #include <boost/thread.hpp>
 40 | #include <sensor_msgs/PointCloud.h>
 41 | #include <sensor_msgs/PointCloud2.h>
 42 | #include <sensor_msgs/point_cloud_conversion.h>
 43 | 
 44 | using namespace std;
 45 | 
 46 | class PointCloudConverter 
 47 | {
 48 |   private:
 49 |     boost::mutex m_mutex_;
 50 |     // ROS
 51 |     ros::NodeHandle nh_;
 52 |     ros::Subscriber sub_points_, sub_points2_;
 53 |     ros::Publisher pub_points_, pub_points2_;
 54 | 
 55 |     int queue_size_;
 56 |     string points_in_, points2_in_, points_out_, points2_out_;
 57 | 
 58 |   public:
 59 |     PointCloudConverter () : nh_ ("~"), queue_size_ (100), 
 60 |                              points_in_ ("/points_in"), points2_in_ ("/points2_in"), 
 61 |                              points_out_ ("/points_out"), points2_out_ ("/points2_out")
 62 |     {
 63 |       // Subscribe to the cloud topic using both the old message format and the new
 64 |       sub_points_ = nh_.subscribe (points_in_, queue_size_, &PointCloudConverter::cloud_cb_points, this);
 65 |       sub_points2_ = nh_.subscribe (points2_in_, queue_size_, &PointCloudConverter::cloud_cb_points2, this);
 66 |       pub_points_ = nh_.advertise<sensor_msgs::PointCloud> (points_out_, queue_size_);
 67 |       pub_points2_ = nh_.advertise<sensor_msgs::PointCloud2> (points2_out_, queue_size_);
 68 |       ROS_INFO ("PointCloudConverter initialized to transform from PointCloud (%s) to PointCloud2 (%s).", nh_.resolveName (points_in_).c_str (), nh_.resolveName (points2_out_).c_str ());
 69 |       ROS_INFO ("PointCloudConverter initialized to transform from PointCloud2 (%s) to PointCloud (%s).", nh_.resolveName (points2_in_).c_str (), nh_.resolveName (points_out_).c_str ());
 70 |     }
 71 |     
 72 | 
 73 |     inline std::string
 74 |       getFieldsList (const sensor_msgs::PointCloud2 &cloud)
 75 |     {
 76 |       std::string result;
 77 |       for (size_t i = 0; i < cloud.fields.size () - 1; ++i)
 78 |         result += cloud.fields[i].name + " ";
 79 |       result += cloud.fields[cloud.fields.size () - 1].name;
 80 |       return (result);
 81 |     }
 82 | 
 83 | 
 84 |     inline std::string
 85 |       getFieldsList (const sensor_msgs::PointCloud &points)
 86 |     {
 87 |       std::string result = "x y z";
 88 |       for (size_t i = 0; i < points.channels.size (); i++)
 89 |         result = result + " " + points.channels[i].name;
 90 |       return (result);
 91 |     }
 92 | 
 93 | 
 94 |     void
 95 |       cloud_cb_points2 (const sensor_msgs::PointCloud2ConstPtr &msg)
 96 |     {
 97 |       if (pub_points_.getNumSubscribers () <= 0)
 98 |       {
 99 |         //ROS_DEBUG ("[point_cloud_converter] Got a PointCloud2 with %d points on %s, but no subscribers.", msg->width * msg->height, nh_.resolveName (points2_in_).c_str ());
100 |         return;
101 |       }
102 | 
103 |       ROS_DEBUG ("[point_cloud_converter] Got a PointCloud2 with %d points (%s) on %s.", msg->width * msg->height, getFieldsList (*msg).c_str (), nh_.resolveName (points2_in_).c_str ());
104 |       
105 |       sensor_msgs::PointCloud output;
106 |       // Convert to the new PointCloud format
107 |       if (!sensor_msgs::convertPointCloud2ToPointCloud (*msg, output))
108 |       {
109 |         ROS_ERROR ("[point_cloud_converter] Conversion from sensor_msgs::PointCloud2 to sensor_msgs::PointCloud failed!");
110 |         return;
111 |       }
112 |       ROS_DEBUG ("[point_cloud_converter] Publishing a PointCloud with %d points on %s.", (int)output.points.size (), nh_.resolveName (points_out_).c_str ());
113 |       pub_points_.publish (output);
114 |     }
115 | 
116 | 
117 |     void
118 |       cloud_cb_points (const sensor_msgs::PointCloudConstPtr &msg)
119 |     {
120 |       if (pub_points2_.getNumSubscribers () <= 0)
121 |       {
122 |         //ROS_DEBUG ("[point_cloud_converter] Got a PointCloud with %d points on %s, but no subscribers.", (int)msg->points.size (), nh_.resolveName (points_in_).c_str ());
123 |         return;
124 |       }
125 | 
126 |       ROS_DEBUG ("[point_cloud_converter] Got a PointCloud with %d points (%s) on %s.", (int)msg->points.size (), getFieldsList (*msg).c_str (), nh_.resolveName (points_in_).c_str ());
127 | 
128 |       sensor_msgs::PointCloud2 output;
129 |       // Convert to the old point cloud format
130 |       if (!sensor_msgs::convertPointCloudToPointCloud2 (*msg, output))
131 |       {
132 |         ROS_ERROR ("[point_cloud_converter] Conversion from sensor_msgs::PointCloud to sensor_msgs::PointCloud2 failed!");
133 |         return;
134 |       }
135 |       ROS_DEBUG ("[point_cloud_converter] Publishing a PointCloud2 with %d points on %s.", output.height * output.width, nh_.resolveName (points2_out_).c_str ());
136 |       pub_points2_.publish (output);
137 |     }
138 | 
139 | };
140 | 
141 | /* ---[ */
142 | int
143 |   main (int argc, char** argv)
144 | {
145 |   // ROS init
146 |   ros::init (argc, argv, "point_cloud_converter", ros::init_options::AnonymousName);
147 | 
148 |   PointCloudConverter p;
149 |   ros::spin ();
150 | 
151 |   return (0);
152 | }


--------------------------------------------------------------------------------