robinJKU · gadringer · Dec 14, 2013 · Dec 14, 2013 · Dec 14, 2013
diff --git a/point_cloud_transformer/CMakeLists.txt b/point_cloud_transformer/CMakeLists.txt
@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(point_cloud_transformer)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  nodelet
+  pcl_ros
+  roscpp
+  rospy
+  sensor_msgs
+  tf
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES PC2TransformerNodelet
+  CATKIN_DEPENDS pcl_ros roscpp rospy sensor_msgs tf nodelet
+  DEPENDS system_lib
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+## Create the nodelet tutorial library
+add_library(PC2TransformerNodelet src/point_cloud_transform.cpp) #nodelet_PC2_transformer
+
+target_link_libraries(PC2TransformerNodelet 
+	${catkin_LIBRARIES}
+)
+
+#if(catkin_EXPORTED_LIBRARIES)
+#  add_dependencies(PC2TransformerNodelet ) # ${catkin_EXPORTED_LIBRARIES})
+#endif()
+
+install(TARGETS PC2TransformerNodelet
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES nodelet_PC2_transformer.xml point_cloud_transformer.launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/point_cloud_transformer/launch/point_cloud_transformer.launch b/point_cloud_transformer/launch/point_cloud_transformer.launch
@@ -0,0 +1,13 @@
+<launch>
+	<include file="$(find turtlebot_gazebo)/launch/turtlebot_empty_world.launch" />
+
+	<!-- id of the source and target frame  -->
+	<arg name="source_frame_id" value="camera_depth_optical_frame"/>
+	<arg name="target_frame_id" value="target_frame"/>
+
+	<node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
+
+	<node pkg="tf" type="static_transform_publisher" name="target_broadcaster" args="1 0 0 0 0 0 $(arg source_frame_id) $(arg target_frame_id) 100" />
+	<node pkg="nodelet" type="nodelet" name="PC2TransformerNodelet" args="load point_cloud_transformer/PC2TransformerNodelet standalone_nodelet" output="screen">
+	</node>  
+</launch>
diff --git a/point_cloud_transformer/nodelet_PC2_transformer.xml b/point_cloud_transformer/nodelet_PC2_transformer.xml
@@ -0,0 +1,7 @@
+<library path="lib/libPC2TransformerNodelet">
+  <class name="point_cloud_transformer/PC2TransformerNodelet" type="point_cloud_transformer::PC2TransformerNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      A node for transforming a PointCloud2
+    </description>
+  </class>
+</library>
diff --git a/point_cloud_transformer/package.xml b/point_cloud_transformer/package.xml
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package>
+  <name>point_cloud_transformer</name>
+  <version>0.0.2</version>
+  <description>Transform a PointCloude2 from frame A to frame B (using a Nodelet)</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="gadringer.stefan@gmail.com">gadringer</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/point_cloud_transformer</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>nodelet</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>tf</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+    <nodelet plugin="${prefix}/nodelet_PC2_transformer.xml"/>
+  </export>
+</package>
diff --git a/point_cloud_transformer/src/point_cloud_transform.cpp b/point_cloud_transformer/src/point_cloud_transform.cpp
@@ -0,0 +1,65 @@
+#include <ros/ros.h>
+
+//This header allows you to publish and subscribe pcl::PointCloud<T> objects as ROS message
+#include <pcl_ros/point_cloud.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_ros/transforms.h>
+#include <nodelet/nodelet.h>
+
+namespace point_cloud_transformer{
+
+class PC2TransformerNodelet : public nodelet::Nodelet{
+	public:
+		sensor_msgs::PointCloud2 cloud_transformed, cloud_income;
+		bool is_cloud_subscribed;
+		std::string target_frame_name;
+
+
+		PC2TransformerNodelet() {}; //:is_cloud_subscribed(false)
+		~PC2TransformerNodelet() {}
+
+		void cmdCallback(const sensor_msgs::PointCloud2::ConstPtr &msg){
+			cloud_income = *msg;
+			is_cloud_subscribed = true;
+		}
+  private:
+
+		virtual void onInit(){
+
+		ros::NodeHandle n = this->getPrivateNodeHandle();
+		is_cloud_subscribed = false;
+		ros::Publisher point_cloud_pub = n.advertise<sensor_msgs::PointCloud2>("/cloud_transformed", 100); //params: topic, size of the message queue
+
+		// Create a subscriber.
+		// Name the topic, message queue, callback function with class name, and object containing callback function.
+		ros::Subscriber point_cloud_sub = n.subscribe("/camera/depth/points", 100, &PC2TransformerNodelet::cmdCallback, this);
+		tf::TransformListener listener; ///cloud_throttled
+
+		ros::Rate rate(5.0);
+		while(n.ok()){
+		    ros::spinOnce(); // for callbacks
+		    if(is_cloud_subscribed){
+		    	try{
+		       pcl_ros::transformPointCloud ("target_frame",cloud_income,cloud_transformed,listener); 
+		       //target_frame_name
+		       point_cloud_pub.publish(cloud_transformed);
+		    	 //   cloud_subscribed = false;
+		    	}
+		    	catch(tf::TransformException e){
+		    		ROS_INFO("Transform_PointCloud: %s", e.what());
+		    	}
+		    }
+
+		  rate.sleep();
+		}
+	}
+};
+
+#include <pluginlib/class_list_macros.h>
+//PLUGINLIB_DECLARE_CLASS(point_cloud_transformer, PC2TransformerNodelet, point_cloud_transformer::PC2TransformerNodelet, nodelet::Nodelet) 
+
+PLUGINLIB_EXPORT_CLASS(point_cloud_transformer::PC2TransformerNodelet,
+                       nodelet::Nodelet);
+}
+
+
diff --git a/point_cloud_transformer_test/CMakeLists.txt b/point_cloud_transformer_test/CMakeLists.txt
@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(point_cloud_transformer_test)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  pcl_ros
+  roscpp
+  rospy
+  sensor_msgs
+  tf
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES point_cloud_transformer_test
+  CATKIN_DEPENDS pcl_ros roscpp rospy sensor_msgs tf
+  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(point_cloud_trans src/point_cloud_trans.cpp)
+target_link_libraries(point_cloud_trans ${catkin_LIBRARIES})
+add_dependencies(point_cloud_trans ${catkin_EXPORTED_TARGETS}) #depend on all necessary targets