diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt
index 9daab4a..5cfdf87 100644
--- a/cartographer_ros/CMakeLists.txt
+++ b/cartographer_ros/CMakeLists.txt
@@ -132,29 +132,20 @@ set_target_properties(${PROJECT_NAME} PROPERTIES
   COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
 
 if (CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(configuration_files_test "cartographer_ros/configuration_files_test.cc")
-  # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to
-  # target_link_libraries. That forces us to do the same.
-  target_include_directories(configuration_files_test SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
-  target_link_libraries(configuration_files_test ${LUA_LIBRARIES})
-  target_include_directories(configuration_files_test SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
-  target_link_libraries(configuration_files_test ${catkin_LIBRARIES})
-  add_dependencies(configuration_files_test ${catkin_EXPORTED_TARGETS})
-  target_link_libraries(configuration_files_test cartographer)
-  target_link_libraries(configuration_files_test ${PROJECT_NAME})
-  set_target_properties(configuration_files_test PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
-
-  catkin_add_gtest(time_conversion_test "cartographer_ros/time_conversion_test.cc")
-  # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to
-  # target_link_libraries. That forces us to do the same.
-  target_include_directories(time_conversion_test SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
-  target_link_libraries(time_conversion_test ${LUA_LIBRARIES})
-  target_include_directories(time_conversion_test SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
-  target_link_libraries(time_conversion_test ${catkin_LIBRARIES})
-  add_dependencies(time_conversion_test ${catkin_EXPORTED_TARGETS})
-  target_link_libraries(time_conversion_test cartographer)
-  target_link_libraries(time_conversion_test ${PROJECT_NAME})
-  set_target_properties(time_conversion_test PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
+  foreach(TEST_SOURCE_FILENAME ${ALL_TESTS})
+    get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE)
+    catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME})
+    # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to
+    # target_link_libraries. That forces us to do the same.
+    target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
+    target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES})
+    target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
+    target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES})
+    add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS})
+    target_link_libraries(${TEST_NAME} cartographer)
+    target_link_libraries(${TEST_NAME} ${PROJECT_NAME})
+    set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
+  endforeach()
 endif()
 
 install(DIRECTORY launch urdf configuration_files
diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc
index 9ff3939..a92e5e4 100644
--- a/cartographer_ros/cartographer_ros/assets_writer.cc
+++ b/cartographer_ros/cartographer_ros/assets_writer.cc
@@ -55,16 +55,19 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
 
   carto::io::NullPointsProcessor null_points_processor;
   carto::io::XRayPointsProcessor xy_xray_points_processor(
-      voxel_size, carto::transform::Rigid3f::Rotation(
-                      Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
+      voxel_size,
+      carto::transform::Rigid3f::Rotation(
+          Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
       {}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
   carto::io::XRayPointsProcessor yz_xray_points_processor(
-      voxel_size, carto::transform::Rigid3f::Rotation(
-                      Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
+      voxel_size,
+      carto::transform::Rigid3f::Rotation(
+          Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
       {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
   carto::io::XRayPointsProcessor xz_xray_points_processor(
-      voxel_size, carto::transform::Rigid3f::Rotation(
-                      Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
+      voxel_size,
+      carto::transform::Rigid3f::Rotation(
+          Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
       {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
   carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
       file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc
index 0b16db5..fb0385f 100644
--- a/cartographer_ros/cartographer_ros/assets_writer_main.cc
+++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc
@@ -66,30 +66,6 @@ namespace {
 
 namespace carto = ::cartographer;
 
-carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
-    const sensor_msgs::PointCloud2::ConstPtr& message) {
-  pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
-  pcl::fromROSMsg(*message, pcl_point_cloud);
-  carto::sensor::PointCloudWithIntensities point_cloud;
-
-  // TODO(hrapp): How to get reflectivities from PCL?
-  for (const auto& point : pcl_point_cloud) {
-    point_cloud.points.emplace_back(point.x, point.y, point.z);
-    point_cloud.intensities.push_back(1.);
-  }
-  return point_cloud;
-}
-
-carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
-    const sensor_msgs::MultiEchoLaserScan::ConstPtr& message) {
-  return carto::sensor::ToPointCloudWithIntensities(ToCartographer(*message));
-}
-
-carto::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
-    const sensor_msgs::LaserScan::ConstPtr& message) {
-  return carto::sensor::ToPointCloudWithIntensities(ToCartographer(*message));
-}
-
 template <typename T>
 void HandleMessage(
     const T& message, const string& tracking_frame,
@@ -97,7 +73,7 @@ void HandleMessage(
     const carto::transform::TransformInterpolationBuffer&
         transform_interpolation_buffer,
     const std::vector<std::unique_ptr<carto::io::PointsProcessor>>& pipeline) {
-  const carto::common::Time time = FromRos(message->header.stamp);
+  const carto::common::Time time = FromRos(message.header.stamp);
   if (!transform_interpolation_buffer.Has(time)) {
     return;
   }
@@ -106,14 +82,14 @@ void HandleMessage(
       transform_interpolation_buffer.Lookup(time);
   const carto::transform::Rigid3d sensor_to_tracking =
       ToRigid3d(tf_buffer.lookupTransform(
-          tracking_frame, message->header.frame_id, message->header.stamp));
+          tracking_frame, message.header.frame_id, message.header.stamp));
   const carto::transform::Rigid3f sensor_to_map =
       (tracking_to_map * sensor_to_tracking).cast<float>();
 
   auto batch = carto::common::make_unique<carto::io::PointsBatch>();
   batch->time = time;
   batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
-  batch->frame_id = message->header.frame_id;
+  batch->frame_id = message.header.frame_id;
 
   carto::sensor::PointCloudWithIntensities point_cloud =
       ToPointCloudWithIntensities(message);
@@ -179,17 +155,17 @@ void Run(const string& trajectory_filename, const string& bag_filename,
 
     for (const rosbag::MessageInstance& message : view) {
       if (message.isType<sensor_msgs::PointCloud2>()) {
-        HandleMessage(message.instantiate<sensor_msgs::PointCloud2>(),
+        HandleMessage(*message.instantiate<sensor_msgs::PointCloud2>(),
                       tracking_frame, *tf_buffer,
                       *transform_interpolation_buffer, pipeline);
       }
       if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
-        HandleMessage(message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
+        HandleMessage(*message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
                       tracking_frame, *tf_buffer,
                       *transform_interpolation_buffer, pipeline);
       }
       if (message.isType<sensor_msgs::LaserScan>()) {
-        HandleMessage(message.instantiate<sensor_msgs::LaserScan>(),
+        HandleMessage(*message.instantiate<sensor_msgs::LaserScan>(),
                       tracking_frame, *tf_buffer,
                       *transform_interpolation_buffer, pipeline);
       }
diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc
index fef34fa..19c0615 100644
--- a/cartographer_ros/cartographer_ros/msg_conversion.cc
+++ b/cartographer_ros/cartographer_ros/msg_conversion.cc
@@ -18,7 +18,6 @@
 
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
-#include "cartographer/sensor/proto/sensor.pb.h"
 #include "cartographer/transform/proto/transform.pb.h"
 #include "cartographer/transform/transform.h"
 #include "cartographer_ros/time_conversion.h"
@@ -46,6 +45,7 @@ constexpr float kPointCloudComponentFourMagic = 1.;
 
 using ::cartographer::transform::Rigid3d;
 using ::cartographer::kalman_filter::PoseCovariance;
+using ::cartographer::sensor::PointCloudWithIntensities;
 
 sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp,
                                                    const string& frame_id,
@@ -76,76 +76,58 @@ sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp,
   return msg;
 }
 
+// For sensor_msgs::LaserScan.
+bool HasEcho(float) { return true; }
+
+float GetFirstEcho(float range) { return range; }
+
+// For sensor_msgs::MultiEchoLaserScan.
+bool HasEcho(const sensor_msgs::LaserEcho& echo) {
+  return !echo.echoes.empty();
+}
+
+float GetFirstEcho(const sensor_msgs::LaserEcho& echo) {
+  return echo.echoes[0];
+}
+
+// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
+template <typename LaserMessageType>
+PointCloudWithIntensities LaserScanToPointCloudWithIntensities(
+    const LaserMessageType& msg) {
+  CHECK_GE(msg.range_min, 0.f);
+  CHECK_GE(msg.range_max, msg.range_min);
+  if (msg.angle_increment > 0.f) {
+    CHECK_GT(msg.angle_max, msg.angle_min);
+  } else {
+    CHECK_GT(msg.angle_min, msg.angle_max);
+  }
+  PointCloudWithIntensities point_cloud;
+  float angle = msg.angle_min;
+  for (size_t i = 0; i < msg.ranges.size(); ++i) {
+    const auto& echoes = msg.ranges[i];
+    if (HasEcho(echoes)) {
+      const float first_echo = GetFirstEcho(echoes);
+      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
+        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
+        point_cloud.points.push_back(rotation *
+                                     (first_echo * Eigen::Vector3f::UnitX()));
+        if (msg.intensities.size() > 0) {
+          CHECK_EQ(msg.intensities.size(), msg.ranges.size());
+          const auto& echo_intensities = msg.intensities[i];
+          CHECK(HasEcho(echo_intensities));
+          point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
+        } else {
+          point_cloud.intensities.push_back(0.f);
+        }
+      }
+    }
+    angle += msg.angle_increment;
+  }
+  return point_cloud;
+}
+
 }  // namespace
 
-sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
-    const int64 timestamp, const string& frame_id,
-    const ::cartographer::sensor::proto::LaserScan& laser_scan) {
-  sensor_msgs::MultiEchoLaserScan msg;
-  msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
-  msg.header.frame_id = frame_id;
-
-  msg.angle_min = laser_scan.angle_min();
-  msg.angle_max = laser_scan.angle_max();
-  msg.angle_increment = laser_scan.angle_increment();
-  msg.time_increment = laser_scan.time_increment();
-  msg.scan_time = laser_scan.scan_time();
-  msg.range_min = laser_scan.range_min();
-  msg.range_max = laser_scan.range_max();
-
-  for (const auto& echoes : laser_scan.range()) {
-    msg.ranges.emplace_back();
-    std::copy(echoes.value().begin(), echoes.value().end(),
-              std::back_inserter(msg.ranges.back().echoes));
-  }
-
-  for (const auto& echoes : laser_scan.intensity()) {
-    msg.intensities.emplace_back();
-    std::copy(echoes.value().begin(), echoes.value().end(),
-              std::back_inserter(msg.intensities.back().echoes));
-  }
-  return msg;
-}
-
-sensor_msgs::LaserScan ToLaserScan(
-    const int64 timestamp, const string& frame_id,
-    const ::cartographer::sensor::proto::LaserScan& laser_scan) {
-  sensor_msgs::LaserScan msg;
-  msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
-  msg.header.frame_id = frame_id;
-
-  msg.angle_min = laser_scan.angle_min();
-  msg.angle_max = laser_scan.angle_max();
-  msg.angle_increment = laser_scan.angle_increment();
-  msg.time_increment = laser_scan.time_increment();
-  msg.scan_time = laser_scan.scan_time();
-  msg.range_min = laser_scan.range_min();
-  msg.range_max = laser_scan.range_max();
-
-  for (const auto& echoes : laser_scan.range()) {
-    if (echoes.value_size() > 0) {
-      msg.ranges.push_back(echoes.value(0));
-    } else {
-      msg.ranges.push_back(0.);
-    }
-  }
-
-  bool has_intensities = false;
-  for (const auto& echoes : laser_scan.intensity()) {
-    if (echoes.value_size() > 0) {
-      msg.intensities.push_back(echoes.value(0));
-      has_intensities = true;
-    } else {
-      msg.intensities.push_back(0);
-    }
-  }
-  if (!has_intensities) {
-    msg.intensities.clear();
-  }
-
-  return msg;
-}
-
 sensor_msgs::PointCloud2 ToPointCloud2Message(
     const int64 timestamp, const string& frame_id,
     const ::cartographer::sensor::PointCloud& point_cloud) {
@@ -160,73 +142,28 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
   return msg;
 }
 
-sensor_msgs::PointCloud2 ToPointCloud2Message(
-    const int64 timestamp, const string& frame_id,
-    const ::cartographer::sensor::proto::RangeData& range_data) {
-  CHECK(::cartographer::transform::ToEigen(range_data.origin()).norm() == 0)
-      << "Trying to convert a range_data that is not at the origin.";
-
-  const auto& point_cloud = range_data.point_cloud();
-  CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
-  CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
-  const auto num_points = point_cloud.x_size();
-
-  auto msg = PreparePointCloud2Message(timestamp, frame_id, num_points);
-  ::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
-  for (int i = 0; i < num_points; ++i) {
-    stream.next(point_cloud.x(i));
-    stream.next(point_cloud.y(i));
-    stream.next(point_cloud.z(i));
-    stream.next(kPointCloudComponentFourMagic);
-  }
-  return msg;
-}
-
-::cartographer::sensor::proto::LaserScan ToCartographer(
+PointCloudWithIntensities ToPointCloudWithIntensities(
     const sensor_msgs::LaserScan& msg) {
-  ::cartographer::sensor::proto::LaserScan proto;
-  proto.set_angle_min(msg.angle_min);
-  proto.set_angle_max(msg.angle_max);
-  proto.set_angle_increment(msg.angle_increment);
-  proto.set_time_increment(msg.time_increment);
-  proto.set_scan_time(msg.scan_time);
-  proto.set_range_min(msg.range_min);
-  proto.set_range_max(msg.range_max);
-
-  for (const auto& range : msg.ranges) {
-    proto.add_range()->mutable_value()->Add(range);
-  }
-
-  for (const auto& intensity : msg.intensities) {
-    proto.add_intensity()->mutable_value()->Add(intensity);
-  }
-  return proto;
+  return LaserScanToPointCloudWithIntensities(msg);
 }
 
-::cartographer::sensor::proto::LaserScan ToCartographer(
+PointCloudWithIntensities ToPointCloudWithIntensities(
     const sensor_msgs::MultiEchoLaserScan& msg) {
-  ::cartographer::sensor::proto::LaserScan proto;
-  proto.set_angle_min(msg.angle_min);
-  proto.set_angle_max(msg.angle_max);
-  proto.set_angle_increment(msg.angle_increment);
-  proto.set_time_increment(msg.time_increment);
-  proto.set_scan_time(msg.scan_time);
-  proto.set_range_min(msg.range_min);
-  proto.set_range_max(msg.range_max);
+  return LaserScanToPointCloudWithIntensities(msg);
+}
 
-  for (const auto& range : msg.ranges) {
-    auto* proto_echoes = proto.add_range()->mutable_value();
-    for (const auto& echo : range.echoes) {
-      proto_echoes->Add(echo);
-    }
+PointCloudWithIntensities ToPointCloudWithIntensities(
+    const sensor_msgs::PointCloud2& message) {
+  pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
+  pcl::fromROSMsg(message, pcl_point_cloud);
+  PointCloudWithIntensities point_cloud;
+
+  // TODO(hrapp): How to get reflectivities from PCL?
+  for (const auto& point : pcl_point_cloud) {
+    point_cloud.points.emplace_back(point.x, point.y, point.z);
+    point_cloud.intensities.push_back(1.);
   }
-  for (const auto& intensity : msg.intensities) {
-    auto* proto_echoes = proto.add_intensity()->mutable_value();
-    for (const auto& echo : intensity.echoes) {
-      proto_echoes->Add(echo);
-    }
-  }
-  return proto;
+  return point_cloud;
 }
 
 Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {
diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h
index 91e564a..c7dae2c 100644
--- a/cartographer_ros/cartographer_ros/msg_conversion.h
+++ b/cartographer_ros/cartographer_ros/msg_conversion.h
@@ -20,7 +20,6 @@
 #include "cartographer/common/port.h"
 #include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/sensor/point_cloud.h"
-#include "cartographer/sensor/proto/sensor.pb.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "geometry_msgs/Pose.h"
 #include "geometry_msgs/Transform.h"
@@ -35,35 +34,25 @@
 
 namespace cartographer_ros {
 
-// Returns a laser scan consisting of the first echo of each beam.
-sensor_msgs::LaserScan ToLaserScan(
-    int64 timestamp, const string& frame_id,
-    const ::cartographer::sensor::proto::LaserScan& laser_scan);
-
-sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
-    int64 timestamp, const string& frame_id,
-    const ::cartographer::sensor::proto::LaserScan& laser_scan);
-
 sensor_msgs::PointCloud2 ToPointCloud2Message(
     int64 timestamp, const string& frame_id,
     const ::cartographer::sensor::PointCloud& point_cloud);
 
-sensor_msgs::PointCloud2 ToPointCloud2Message(
-    int64 timestamp, const string& frame_id,
-    const ::cartographer::sensor::proto::RangeData& range_data);
-
 geometry_msgs::Transform ToGeometryMsgTransform(
     const ::cartographer::transform::Rigid3d& rigid3d);
 
 geometry_msgs::Pose ToGeometryMsgPose(
     const ::cartographer::transform::Rigid3d& rigid3d);
 
-::cartographer::sensor::proto::LaserScan ToCartographer(
+::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
     const sensor_msgs::LaserScan& msg);
 
-::cartographer::sensor::proto::LaserScan ToCartographer(
+::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
     const sensor_msgs::MultiEchoLaserScan& msg);
 
+::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
+    const sensor_msgs::PointCloud2& message);
+
 ::cartographer::transform::Rigid3d ToRigid3d(
     const geometry_msgs::TransformStamped& transform);
 
diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc
new file mode 100644
index 0000000..92b63a0
--- /dev/null
+++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_ros/msg_conversion.h"
+
+#include <cmath>
+
+#include "gtest/gtest.h"
+#include "sensor_msgs/LaserScan.h"
+
+namespace cartographer_ros {
+namespace {
+
+TEST(MsgConversion, LaserScanToPointCloud) {
+  sensor_msgs::LaserScan laser_scan;
+  for (int i = 0; i < 8; ++i) {
+    laser_scan.ranges.push_back(1.f);
+  }
+  laser_scan.angle_min = 0.f;
+  laser_scan.angle_max = 8.f * static_cast<float>(M_PI_4);
+  laser_scan.angle_increment = static_cast<float>(M_PI_4);
+  laser_scan.range_min = 0.f;
+  laser_scan.range_max = 10.f;
+
+  const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points;
+  EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[1].isApprox(
+      Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[3].isApprox(
+      Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[5].isApprox(
+      Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
+      1e-6));
+  EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[7].isApprox(
+      Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6));
+}
+
+TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
+  sensor_msgs::LaserScan laser_scan;
+  laser_scan.ranges.push_back(1.f);
+  laser_scan.ranges.push_back(std::numeric_limits<float>::infinity());
+  laser_scan.ranges.push_back(2.f);
+  laser_scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
+  laser_scan.ranges.push_back(3.f);
+  laser_scan.angle_min = 0.f;
+  laser_scan.angle_max = 3.f * static_cast<float>(M_PI_4);
+  laser_scan.angle_increment = static_cast<float>(M_PI_4);
+  laser_scan.range_min = 2.f;
+  laser_scan.range_max = 10.f;
+
+  const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points;
+  ASSERT_EQ(2, point_cloud.size());
+  EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
+  EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
+}
+
+}  // namespace
+}  // namespace cartographer_ros
diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc
index 5feef1f..5cd5b97 100644
--- a/cartographer_ros/cartographer_ros/sensor_bridge.cc
+++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc
@@ -87,14 +87,14 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
 void SensorBridge::HandleLaserScanMessage(
     const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
   HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
-                    carto::sensor::ToPointCloud(ToCartographer(*msg)));
+                    ToPointCloudWithIntensities(*msg).points);
 }
 
 void SensorBridge::HandleMultiEchoLaserScanMessage(
     const string& sensor_id,
     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
   HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
-                    carto::sensor::ToPointCloud(ToCartographer(*msg)));
+                    ToPointCloudWithIntensities(*msg).points);
 }
 
 void SensorBridge::HandlePointCloud2Message(
diff --git a/cartographer_ros/cartographer_ros/time_conversion_test.cc b/cartographer_ros/cartographer_ros/time_conversion_test.cc
index 4443ca4..5f1b7bf 100644
--- a/cartographer_ros/cartographer_ros/time_conversion_test.cc
+++ b/cartographer_ros/cartographer_ros/time_conversion_test.cc
@@ -23,7 +23,6 @@
 #include "ros/ros.h"
 
 namespace cartographer_ros {
-
 namespace {
 
 TEST(TimeConversion, testToRos) {