diff --git a/CMakeLists.txt b/CMakeLists.txt
index d47230db..d5083c25 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,7 +21,5 @@ install(DIRECTORY include/${PROJECT_NAME}/
 )
 
 # Add gtest based cpp test target
-find_package(GTEST QUIET)
-if(GTEST_FOUND)
-  catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp)
-endif()
+catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl_conversions.cpp)
+target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES})
diff --git a/test/test_pcl_conversions.cpp b/test/test_pcl_conversions.cpp
index 180623f5..df647af2 100644
--- a/test/test_pcl_conversions.cpp
+++ b/test/test_pcl_conversions.cpp
@@ -1,5 +1,111 @@
+#include <string>
+
 #include "gtest/gtest.h"
 
 #include "pcl_conversions/pcl_conversions.h"
 
+namespace {
 
+class PCLConversionTests : public ::testing::Test {
+protected:
+  virtual void SetUp() {
+    pcl_image.header.frame_id = "pcl";
+    pcl_image.height = 1;
+    pcl_image.width = 2;
+    pcl_image.step = 1;
+    pcl_image.is_bigendian = true;
+    pcl_image.encoding = "bgr8";
+    pcl_image.data.resize(2);
+    pcl_image.data[0] = 0x42;
+    pcl_image.data[1] = 0x43;
+
+    pcl_pc2.header.frame_id = "pcl";
+    pcl_pc2.height = 1;
+    pcl_pc2.width = 2;
+    pcl_pc2.point_step = 1;
+    pcl_pc2.row_step = 1;
+    pcl_pc2.is_bigendian = true;
+    pcl_pc2.is_dense = true;
+    pcl_pc2.fields.resize(2);
+    pcl_pc2.fields[0].name = "XYZ";
+    pcl_pc2.fields[0].datatype = pcl_sensor_msgs::PCLPointField::INT8;
+    pcl_pc2.fields[0].count = 3;
+    pcl_pc2.fields[0].offset = 0;
+    pcl_pc2.fields[1].name = "RGB";
+    pcl_pc2.fields[1].datatype = pcl_sensor_msgs::PCLPointField::INT8;
+    pcl_pc2.fields[1].count = 3;
+    pcl_pc2.fields[1].offset = 8 * 3;
+    pcl_pc2.data.resize(2);
+    pcl_pc2.data[0] = 0x42;
+    pcl_pc2.data[1] = 0x43;
+  }
+
+  pcl_sensor_msgs::PCLImage pcl_image;
+  sensor_msgs::Image image;
+
+  pcl_sensor_msgs::PCLPointCloud2 pcl_pc2;
+  sensor_msgs::PointCloud2 pc2;
+};
+
+template<class T>
+void test_image(T &image) {
+  EXPECT_EQ(std::string("pcl"), image.header.frame_id);
+  EXPECT_EQ(1, image.height);
+  EXPECT_EQ(2, image.width);
+  EXPECT_EQ(1, image.step);
+  EXPECT_TRUE(image.is_bigendian);
+  EXPECT_EQ(std::string("bgr8"), image.encoding);
+  EXPECT_EQ(2, image.data.size());
+  EXPECT_EQ(0x42, image.data[0]);
+  EXPECT_EQ(0x43, image.data[1]);
+}
+
+TEST_F(PCLConversionTests, imageConversion) {
+  pcl_conversions::fromPCLImageToImage(pcl_image, image);
+  test_image(image);
+  pcl_sensor_msgs::PCLImage pcl_image2;
+  pcl_conversions::fromImageToPCLImage(image, pcl_image2);
+  test_image(pcl_image2);
+}
+
+template<class T>
+void test_pc(T &pc) {
+  EXPECT_EQ(std::string("pcl"), pc.header.frame_id);
+  EXPECT_EQ(1, pc.height);
+  EXPECT_EQ(2, pc.width);
+  EXPECT_EQ(1, pc.point_step);
+  EXPECT_EQ(1, pc.row_step);
+  EXPECT_TRUE(pc.is_bigendian);
+  EXPECT_TRUE(pc.is_dense);
+  EXPECT_EQ("XYZ", pc.fields[0].name);
+  EXPECT_EQ(pcl_sensor_msgs::PCLPointField::INT8, pc.fields[0].datatype);
+  EXPECT_EQ(3, pc.fields[0].count);
+  EXPECT_EQ(0, pc.fields[0].offset);
+  EXPECT_EQ("RGB", pc.fields[1].name);
+  EXPECT_EQ(pcl_sensor_msgs::PCLPointField::INT8, pc.fields[1].datatype);
+  EXPECT_EQ(3, pc.fields[1].count);
+  EXPECT_EQ(8 * 3, pc.fields[1].offset);
+  EXPECT_EQ(2, pc.data.size());
+  EXPECT_EQ(0x42, pc.data[0]);
+  EXPECT_EQ(0x43, pc.data[1]);
+}
+
+TEST_F(PCLConversionTests, pointcloud2Conversion) {
+  pcl_conversions::fromPCLPointCloud2ToPointCloud2(pcl_pc2, pc2);
+  test_pc(pc2);
+  pcl_sensor_msgs::PCLPointCloud2 pcl_pc2_2;
+  pcl_conversions::fromPointCloud2ToPCLPointCloud2(pc2, pcl_pc2_2);
+  test_pc(pcl_pc2_2);
+}
+
+} // namespace
+
+int main(int argc, char **argv) {
+  try {
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+  } catch (std::exception &e) {
+    std::cerr << "Unhandled Exception: " << e.what() << std::endl;
+  }
+  return 1;
+}