Moves SensorData struct into Cartographer. (#110)

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -63,17 +63,10 @@ google_library(sensor_bridge
     sensor_bridge.h
   DEPENDS
     msg_conversion
-    sensor_data
     tf_bridge
     time_conversion
 )
 
-google_library(sensor_data
-  USES_CARTOGRAPHER
-  HDRS
-    sensor_data.h
-)
-
 google_library(tf_bridge
   USES_CARTOGRAPHER
   SRCS
@@ -118,7 +111,6 @@ google_binary(cartographer_node
     occupancy_grid
     ros_log_sink
     sensor_bridge
-    sensor_data
     tf_bridge
     time_conversion
 )

cartographer_ros/cartographer_ros/node_main.cc

@@ -41,6 +41,7 @@
 #include "cartographer/mapping_3d/local_trajectory_builder.h"
 #include "cartographer/mapping_3d/local_trajectory_builder_options.h"
 #include "cartographer/mapping_3d/sparse_pose_graph.h"
+#include "cartographer/sensor/data.h"
 #include "cartographer/sensor/laser.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/sensor/proto/sensor.pb.h"
@@ -52,7 +53,6 @@
 #include "cartographer_ros/occupancy_grid.h"
 #include "cartographer_ros/ros_log_sink.h"
 #include "cartographer_ros/sensor_bridge.h"
-#include "cartographer_ros/sensor_data.h"
 #include "cartographer_ros/tf_bridge.h"
 #include "cartographer_ros/time_conversion.h"
 #include "cartographer_ros_msgs/FinishTrajectory.h"
@@ -121,7 +121,7 @@ class Node {
 
  private:
   void HandleSensorData(int64 timestamp,
-                        std::unique_ptr<SensorData> sensor_data);
+                        std::unique_ptr<carto::sensor::Data> sensor_data);
   bool HandleSubmapQuery(
       ::cartographer_ros_msgs::SubmapQuery::Request& request,
       ::cartographer_ros_msgs::SubmapQuery::Response& response);
@@ -145,7 +145,7 @@ class Node {
   std::deque<carto::mapping::TrajectoryNode::ConstantData> constant_data_
       GUARDED_BY(mutex_);
   carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
-  carto::mapping::SensorCollator<SensorData> sensor_collator_
+  carto::mapping::SensorCollator<carto::sensor::Data> sensor_collator_
       GUARDED_BY(mutex_);
   SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
 
@@ -266,7 +266,8 @@ void Node::Initialize() {
   CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
   sensor_collator_.AddTrajectory(
       kTrajectoryBuilderId, expected_sensor_identifiers,
-      [this](const int64 timestamp, std::unique_ptr<SensorData> sensor_data) {
+      [this](const int64 timestamp,
+             std::unique_ptr<carto::sensor::Data> sensor_data) {
         HandleSensorData(timestamp, std::move(sensor_data));
       });
 
@@ -481,7 +482,7 @@ void Node::SpinOccupancyGridThreadForever() {
 }
 
 void Node::HandleSensorData(const int64 timestamp,
-                            std::unique_ptr<SensorData> sensor_data) {
+                            std::unique_ptr<carto::sensor::Data> sensor_data) {
   auto it = rate_timers_.find(sensor_data->frame_id);
   if (it == rate_timers_.end()) {
     it = rate_timers_
@@ -505,12 +506,12 @@ void Node::HandleSensorData(const int64 timestamp,
   auto* const trajectory_builder =
       map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
   switch (sensor_data->type) {
-    case SensorType::kImu:
+    case carto::sensor::Data::Type::kImu:
       trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration,
                                      sensor_data->imu.angular_velocity);
       return;
 
-    case SensorType::kLaserFan3D:
+    case carto::sensor::Data::Type::kLaserFan3D:
       if (options_.map_builder_options.use_trajectory_builder_2d()) {
         trajectory_builder->AddHorizontalLaserFan(time,
                                                   sensor_data->laser_fan_3d);
@@ -520,7 +521,7 @@ void Node::HandleSensorData(const int64 timestamp,
       }
       return;
 
-    case SensorType::kOdometry:
+    case carto::sensor::Data::Type::kOdometry:
       trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose,
                                           sensor_data->odometry.covariance);
       return;

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -26,6 +26,17 @@ namespace carto = ::cartographer;
 
 using carto::transform::Rigid3d;
 
+namespace {
+
+const string& CheckNoLeadingSlash(const string& frame_id) {
+  if (frame_id.size() > 0) {
+    CHECK_NE(frame_id[0], '/');
+  }
+  return frame_id;
+}
+
+}  // namespace
+
 SensorBridgeOptions CreateSensorBridgeOptions(
     carto::common::LuaParameterDictionary* const lua_parameter_dictionary) {
   SensorBridgeOptions options;
@@ -48,7 +59,7 @@ SensorBridgeOptions CreateSensorBridgeOptions(
 SensorBridge::SensorBridge(
     const SensorBridgeOptions& options, const TfBridge* const tf_bridge,
     const int trajectory_id,
-    carto::mapping::SensorCollator<SensorData>* const sensor_collator)
+    carto::mapping::SensorCollator<carto::sensor::Data>* const sensor_collator)
     : options_(options),
       tf_bridge_(tf_bridge),
       trajectory_id_(trajectory_id),
@@ -74,9 +85,9 @@ void SensorBridge::HandleOdometryMessage(
   if (sensor_to_tracking != nullptr) {
     sensor_collator_->AddSensorData(
         trajectory_id_, carto::common::ToUniversal(time), topic,
-        carto::common::make_unique<SensorData>(
+        carto::common::make_unique<::cartographer::sensor::Data>(
-            msg->child_frame_id,
+            CheckNoLeadingSlash(msg->child_frame_id),
-            SensorData::Odometry{
+            ::cartographer::sensor::Data::Odometry{
                 ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
                 covariance}));
   }
@@ -96,9 +107,9 @@ void SensorBridge::HandleImuMessage(const string& topic,
            "otherwise be imprecise.";
     sensor_collator_->AddSensorData(
         trajectory_id_, carto::common::ToUniversal(time), topic,
-        carto::common::make_unique<SensorData>(
+        carto::common::make_unique<::cartographer::sensor::Data>(
-            msg->header.frame_id,
+            CheckNoLeadingSlash(msg->header.frame_id),
-            SensorData::Imu{
+            ::cartographer::sensor::Data::Imu{
                 sensor_to_tracking->rotation() *
                     ToEigen(msg->linear_acceleration),
                 sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity),
@@ -128,8 +139,8 @@ void SensorBridge::HandlePointCloud2Message(
   if (sensor_to_tracking != nullptr) {
     sensor_collator_->AddSensorData(
         trajectory_id_, carto::common::ToUniversal(time), topic,
-        carto::common::make_unique<SensorData>(
+        carto::common::make_unique<::cartographer::sensor::Data>(
-            msg->header.frame_id,
+            CheckNoLeadingSlash(msg->header.frame_id),
             carto::sensor::TransformLaserFan3D(
                 carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
                 sensor_to_tracking->cast<float>())));
@@ -148,10 +159,11 @@ void SensorBridge::HandleLaserScanProto(
   if (sensor_to_tracking != nullptr) {
     sensor_collator_->AddSensorData(
         trajectory_id_, carto::common::ToUniversal(time), topic,
-        carto::common::make_unique<SensorData>(
+        carto::common::make_unique<::cartographer::sensor::Data>(
-            frame_id, carto::sensor::TransformLaserFan3D(
+            CheckNoLeadingSlash(frame_id),
-                          carto::sensor::ToLaserFan3D(laser_fan_2d),
+            carto::sensor::TransformLaserFan3D(
-                          sensor_to_tracking->cast<float>())));
+                carto::sensor::ToLaserFan3D(laser_fan_2d),
+                sensor_to_tracking->cast<float>())));
   }
 }
 

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -18,9 +18,9 @@
 #define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
 
 #include "cartographer/mapping/sensor_collator.h"
+#include "cartographer/sensor/data.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
-#include "cartographer_ros/sensor_data.h"
 #include "cartographer_ros/tf_bridge.h"
 #include "geometry_msgs/Transform.h"
 #include "geometry_msgs/TransformStamped.h"
@@ -50,7 +50,8 @@ class SensorBridge {
   explicit SensorBridge(
       const SensorBridgeOptions& options, const TfBridge* tf_bridge,
       int trajectory_id,
-      ::cartographer::mapping::SensorCollator<SensorData>* sensor_collator);
+      ::cartographer::mapping::SensorCollator<::cartographer::sensor::Data>*
+          sensor_collator);
 
   SensorBridge(const SensorBridge&) = delete;
   SensorBridge& operator=(const SensorBridge&) = delete;
@@ -76,7 +77,8 @@ class SensorBridge {
   const SensorBridgeOptions options_;
   const TfBridge* const tf_bridge_;
   const int trajectory_id_;
-  ::cartographer::mapping::SensorCollator<SensorData>* const sensor_collator_;
+  ::cartographer::mapping::SensorCollator<::cartographer::sensor::Data>* const
+      sensor_collator_;
 };
 
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/sensor_data.h

@@ -1,76 +0,0 @@
-/*
- * 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.
- */
-
-#ifndef CARTOGRAPHER_ROS_SENSOR_DATA_H_
-#define CARTOGRAPHER_ROS_SENSOR_DATA_H_
-
-#include <string>
-
-#include "cartographer/kalman_filter/pose_tracker.h"
-#include "cartographer/sensor/laser.h"
-#include "cartographer/transform/rigid_transform.h"
-
-namespace cartographer_ros {
-
-// This type is a logical union, i.e. only one type of sensor data is actually
-// filled in. It is only used for time ordering sensor data before passing it
-// on.
-enum class SensorType { kImu, kLaserFan3D, kOdometry };
-struct SensorData {
-  struct Odometry {
-    ::cartographer::transform::Rigid3d pose;
-    ::cartographer::kalman_filter::PoseCovariance covariance;
-  };
-
-  struct Imu {
-    Eigen::Vector3d linear_acceleration;
-    Eigen::Vector3d angular_velocity;
-  };
-
-  SensorData(const string& frame_id, const Imu& imu)
-      : type(SensorType::kImu),
-        frame_id(CheckNoLeadingSlash(frame_id)),
-        imu(imu) {}
-
-  SensorData(const string& frame_id,
-             const ::cartographer::sensor::LaserFan3D& laser_fan_3d)
-      : type(SensorType::kLaserFan3D),
-        frame_id(CheckNoLeadingSlash(frame_id)),
-        laser_fan_3d(laser_fan_3d) {}
-
-  SensorData(const string& frame_id, const Odometry& odometry)
-      : type(SensorType::kOdometry),
-        frame_id(CheckNoLeadingSlash(frame_id)),
-        odometry(odometry) {}
-
-  SensorType type;
-  string frame_id;
-  Imu imu;
-  ::cartographer::sensor::LaserFan3D laser_fan_3d;
-  Odometry odometry;
-
- private:
-  static const string& CheckNoLeadingSlash(const string& frame_id) {
-    if (frame_id.size() > 0) {
-      CHECK_NE(frame_id[0], '/');
-    }
-    return frame_id;
-  }
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_SENSOR_DATA_H_