├── CMakeLists.txt
├── README.md
├── launch
    └── transform.launch
├── nodelet_plugins.xml
├── package.xml
└── src
    └── transform_pointcloud_nodelet.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(transform_pointcloud)
 3 | 
 4 | add_compile_options(-std=c++11)
 5 | 
 6 | find_package(catkin REQUIRED COMPONENTS
 7 |         roscpp
 8 |         sensor_msgs
 9 |         tf
10 |         nodelet
11 |         tf2_sensor_msgs
12 |         )
13 | 
14 | catkin_package(
15 |         #  INCLUDE_DIRS include
16 |         LIBRARIES transformPointcloud
17 |         #  CATKIN_DEPENDS roscpp sensor_msgs tf
18 |         #  DEPENDS system_lib
19 | )
20 | 
21 | ###########
22 | ## Build ##
23 | ###########
24 | 
25 | include_directories(
26 |         include
27 |         ${catkin_INCLUDE_DIRS}
28 | )
29 | 
30 | ## Declare a C++ library
31 | add_library(transformPointcloud
32 |         src/transform_pointcloud_nodelet.cpp
33 |         )
34 | add_dependencies(transformPointcloud ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
35 | target_link_libraries(transformPointcloud
36 |         ${catkin_LIBRARIES}
37 |         )
38 | 
39 | 
40 | install(TARGETS transformPointcloud
41 |         DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
42 | install(DIRECTORY include/${PROJECT_NAME}/
43 |         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
44 | 
45 | install(DIRECTORY plugins
46 |         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
47 | 
48 | install(DIRECTORY launch
49 |         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
50 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # transform_pointcloud #
 2 | ROS nodelet that transforms pointcloud (`sensor_msgs/Pointcloud` or `sensor_msgs/Pointcloud`) to some other 
 3 | TF frame (e.g. from "camera_frame" to "base_link"). It uses `sensor_msgs/point_cloud_conversion.h` for conversion.
 4 | It will publish both `Pointcloud` and `Pointcloud2` messages for any input type (if any node is subscribed to it)
 5 | # Subscribed topics
 6 | * `~input_pcl (sensor_msgs/Pointcloud)` input topic
 7 | * `~inptu_pcl2 (sensor_msgs/Pointcloud2)` input topic
 8 | 
 9 | # Published topics
10 | * `~output_pcl (sensor_msgs/Pointcloud)` output topic
11 | * `~output_pcl2 (sensor_msgs/Pointcloud2)` output topic
12 | 
13 | # Parameters
14 | * `to_frame (string, default:base_link)` to what TF frame to transform the output
15 | 
16 | # TF
17 | * required is the transform from frame that input pointcloud is to frame set by `to_frame` parameter
18 | 
19 | # Usage
20 | See `transform_pointcloud/launch/transform.launch` for example
21 | 


--------------------------------------------------------------------------------
/launch/transform.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <node pkg="nodelet" type="nodelet" name="test_nodelet"  args="manager" output="screen"/>
 3 | 
 4 |     <node pkg="nodelet" type="nodelet" name="pointcloud_transformer" args="load transform_pointcloud/transformPointcloud test_nodelet" output="screen">
 5 |         <param name="to_frame" value="base_link"/>
 6 |         <remap from="~input_pcl2" to="/camera/points"/>
 7 |     </node>
 8 | 
 9 |     <!--<node pkg="nodelet" type="nodelet" name="pointcloud_transformer2" args="load transform_pointcloud/transformPointcloudTafr standalone_nodelet2" output="screen">-->
10 |         <!--<param name="to_frame" value="laser"/>-->
11 |         <!--<remap from="~input_pcl" to="/pointcloud_transformer/output_pcl"/>-->
12 |     <!--</node>-->
13 | 
14 |     <!--<node pkg="nodelet" type="nodelet" name="pointcloud_to_laser" args="load pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet standalone_nodelet2" output="screen">-->
15 |         <!--<remap from="cloud_in" to="/transformed/pointcloud2"/>-->
16 |         <!--<remap from="scan" to="pcl/scan"/>-->
17 |         <!--<param name="min_height" value="-0.15"/>-->
18 |         <!--<param name="max_height" value="0.9"/>-->
19 |         <!--&lt;!&ndash;<param name="angle_min" value=""/>&ndash;&gt;-->
20 |         <!--&lt;!&ndash;<param name="angle_max" value=""/>&ndash;&gt;-->
21 |         <!--<param name="angle_increment" value="0,005817759"/> &lt;!&ndash; 1/3 of degree &ndash;&gt;-->
22 |         <!--<param name="scan_time" value="0,033333333"/>-->
23 |         <!--<param name="range_min" value="0.1"/>-->
24 |         <!--<param name="range_max" value="4"/>-->
25 |         <!--<param name="concurency_level" value="1"/> &lt;!&ndash;n of used cpu cores&ndash;&gt;-->
26 |     <!--</node>-->
27 | </launch>


--------------------------------------------------------------------------------
/nodelet_plugins.xml:
--------------------------------------------------------------------------------
1 | <library path="lib/libtransformPointcloud">
2 |     <class name="transform_pointcloud/transformPointcloud" type="transform_pointcloud::transformPointcloud" base_class_type="nodelet::Nodelet">
3 |         <description>
4 |             Transform pointcloud nodelet
5 |         </description>
6 |     </class>
7 | </library>


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>transform_pointcloud</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The transform_pointcloud package</description>
 6 |   <maintainer email="tim.kambic12@gmail.com">Tim</maintainer>
 7 |   <license>BSD</license>
 8 | 
 9 | 
10 |   <buildtool_depend>catkin</buildtool_depend>
11 |   <build_depend>roscpp</build_depend>
12 |   <build_depend>sensor_msgs</build_depend>
13 |   <build_depend>tf</build_depend>
14 |   <build_depend>tf2_sensor_msgs</build_depend>
15 |   <build_depend>nodelet</build_depend>
16 |   <build_export_depend>roscpp</build_export_depend>
17 |   <build_export_depend>sensor_msgs</build_export_depend>
18 |   <build_export_depend>tf</build_export_depend>
19 |   <exec_depend>roscpp</exec_depend>
20 |   <exec_depend>sensor_msgs</exec_depend>
21 |   <exec_depend>tf</exec_depend>
22 |   <exec_depend>tf2_sensor_msgs</exec_depend>
23 |   <exec_depend>nodelet</exec_depend>
24 | 
25 |   <export>
26 |     <nodelet plugin="${prefix}/nodelet_plugins.xml" />
27 |   </export>
28 | 
29 | 
30 | </package>
31 | 


--------------------------------------------------------------------------------
/src/transform_pointcloud_nodelet.cpp:
--------------------------------------------------------------------------------
  1 | //Copyright (c) 2018, Tim Kambic
  2 | //All rights reserved.
  3 | //
  4 | //Redistribution and use in source and binary forms, with or without
  5 | //modification, are permitted provided that the following conditions are met:
  6 | //* Redistributions of source code must retain the above copyright
  7 | //	notice, this list of conditions and the following disclaimer.
  8 | //* Redistributions in binary form must reproduce the above copyright
  9 | //	notice, this list of conditions and the following disclaimer in the
 10 | //	documentation and/or other materials provided with the distribution.
 11 | //* Neither the name of the <organization> nor the
 12 | //	names of its contributors may be used to endorse or promote products
 13 | //	derived from this software without specific prior written permission.
 14 | //
 15 | //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 16 | //ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 17 | //WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 18 | //DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 19 | //DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 20 | //(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 21 | //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 22 | //ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 23 | //(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 24 | //SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 25 | 
 26 | #include "ros/ros.h"
 27 | #include "nodelet/nodelet.h"
 28 | #include <pluginlib/class_list_macros.h>
 29 | 
 30 | #include <tf/transform_listener.h>
 31 | #include <sensor_msgs/PointCloud2.h>
 32 | #include <sensor_msgs/point_cloud_conversion.h>
 33 | #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
 34 | 
 35 | 
 36 | namespace transform_pointcloud {
 37 | 	class transformPointcloud : public nodelet::Nodelet {
 38 | 		ros::Publisher pc_pub;
 39 | 		ros::Publisher pc2_pub;
 40 | 		ros::Subscriber pc_sub;
 41 | 		ros::Subscriber pc2_sub;
 42 | 
 43 | 		std::string REFERENCE_FRAME = "base_link";
 44 | 
 45 | 	public:
 46 | 		transformPointcloud() = default;
 47 | 
 48 | 	private:
 49 | 		virtual void onInit() {
 50 | 			ros::NodeHandle nh = getNodeHandle();
 51 | 			ros::NodeHandle nhp = getPrivateNodeHandle();
 52 | 			if (!getPrivateNodeHandle().getParam("to_frame", REFERENCE_FRAME)) {
 53 | 				ROS_ERROR("You must specify 'to_frame' parameter !");
 54 | 				exit(-123);
 55 | 			}
 56 | 			pc_pub = nhp.advertise<sensor_msgs::PointCloud>("output_pcl", 1);
 57 | 			pc2_pub = nhp.advertise<sensor_msgs::PointCloud2>("output_pcl2", 1);
 58 | 			pc_sub = nhp.subscribe<sensor_msgs::PointCloud>("input_pcl", 1, &transformPointcloud::pointcloudCB,	this);
 59 | 			pc2_sub = nhp.subscribe<sensor_msgs::PointCloud2>("input_pcl2", 1, &transformPointcloud::pointcloud2CB, this);
 60 | 		};
 61 | 
 62 | 		bool getTransform(const std::string &refFrame, const std::string &childFrame, tf::StampedTransform &transform) {
 63 | 			std::string errMsg;
 64 | 			tf::TransformListener tf_listener;
 65 | 			if (!tf_listener.waitForTransform(refFrame, childFrame, ros::Time(0), ros::Duration(0.5),
 66 | 											  ros::Duration(0.01), &errMsg)) {
 67 | 				ROS_ERROR_STREAM("Pointcloud transform | Unable to get pose from TF: " << errMsg);
 68 | 				return false;
 69 | 			} else {
 70 | 				try {
 71 | 					tf_listener.lookupTransform(refFrame, childFrame, ros::Time(0), transform);
 72 | 				}
 73 | 				catch (const tf::TransformException &e) {
 74 | 					ROS_ERROR_STREAM(
 75 | 							"Pointcloud transform | Error in lookupTransform of " << childFrame << " in " << refFrame);
 76 | 					return false;
 77 | 				}
 78 | 			}
 79 | 			return true;
 80 | 		}
 81 | 
 82 | 		void pointcloudCB(const sensor_msgs::PointCloud::ConstPtr &cloud_in) {
 83 | 			if (cloud_in->points.empty()) { return; }
 84 | 
 85 | 			tf::StampedTransform tf_between_frames;
 86 | 			getTransform(REFERENCE_FRAME, cloud_in->header.frame_id, tf_between_frames); // get transformation
 87 | 			geometry_msgs::TransformStamped tf_between_frames_geo;
 88 | 			tf::transformStampedTFToMsg(tf_between_frames, tf_between_frames_geo); // convert it to geometry_msg type
 89 | 
 90 | 			sensor_msgs::PointCloud2 cloud_in2;
 91 | 			sensor_msgs::convertPointCloudToPointCloud2(*cloud_in, cloud_in2); // convert from pointcloud to pointcloud2
 92 | 			sensor_msgs::PointCloud2 cloud_in_transformed;
 93 | 			tf2::doTransform(cloud_in2, cloud_in_transformed, tf_between_frames_geo); // do transformation
 94 | 			if (pc2_pub.getNumSubscribers() > 0) { // publish pointcloud2
 95 | 				pc2_pub.publish(cloud_in_transformed);
 96 | 			}
 97 | 			if (pc_pub.getNumSubscribers() > 0) {
 98 | 				sensor_msgs::PointCloud cloud_out;
 99 | 				sensor_msgs::convertPointCloud2ToPointCloud(cloud_in_transformed, cloud_out); //convert from pointcloud to pointcloud2
100 | 				pc_pub.publish(cloud_out);  // publish pointcloud
101 | 			}
102 | 		}
103 | 
104 | 		void pointcloud2CB(const sensor_msgs::PointCloud2::ConstPtr &cloud_in) {
105 | 			if (cloud_in->data.empty()) { return; }
106 | 
107 | 			tf::StampedTransform tf_between_frames;
108 | 			getTransform(REFERENCE_FRAME, cloud_in->header.frame_id, tf_between_frames);
109 | 			geometry_msgs::TransformStamped tf_between_frames_geo;
110 | 			tf::transformStampedTFToMsg(tf_between_frames, tf_between_frames_geo);
111 | 
112 | 			sensor_msgs::PointCloud2 cloud_in_transformed;
113 | 			tf2::doTransform(*cloud_in, cloud_in_transformed, tf_between_frames_geo); // do transformation
114 | 			if (pc2_pub.getNumSubscribers() > 0) {
115 | 				pc2_pub.publish(cloud_in_transformed);
116 | 			}
117 | 			if (pc_pub.getNumSubscribers() > 0) {
118 | 				sensor_msgs::PointCloud cloud_out;
119 | 				sensor_msgs::convertPointCloud2ToPointCloud(cloud_in_transformed, cloud_out); //convert from pointcloud to pointcloud2
120 | 				pc_pub.publish(cloud_out);
121 | 			}
122 | 		}
123 | 
124 | 
125 | 	};
126 | }
127 | 
128 | PLUGINLIB_DECLARE_CLASS(transform_pointcloud, transformPointcloud, transform_pointcloud::transformPointcloud, nodelet::Nodelet);
129 | 
130 | 


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