DataGenerator: refactored data generator so that we can easily register and manager sensors.

modules/perception/tool/data_generator/BUILD

@@ -4,8 +4,15 @@ package(default_visibility = ["//visibility:public"])
 
 cc_library(
     name = "data_generator_lib",
-    srcs = ["data_generator.cc"],
-    hdrs = ["data_generator.h", "sensor.h"],
+    srcs = [
+        "data_generator.cc",
+        "velodyne64.cc",
+    ],
+    hdrs = [
+        "data_generator.h",
+        "sensor.h",
+        "velodyne64.h",
+    ],
     copts = [
         "-Wno-deprecated",
     ],

modules/perception/tool/data_generator/data_generator.cc

@@ -83,51 +83,16 @@ void DataGenerator::RunOnce() {
     return;
   }
 
-  const auto& point_cloud_msg =
-      AdapterManager::GetPointCloud()->GetLatestObserved();
-  ADEBUG << "PointCloud: " << point_cloud_msg.header;
-
+  // Update VehicleState
   const auto& localization =
       AdapterManager::GetLocalization()->GetLatestObserved();
   ADEBUG << "Localization: " << localization.DebugString();
-
   const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
   ADEBUG << "Chassis: " << chassis.DebugString();
-
   VehicleStateProvider::instance()->Update(localization, chassis);
   AINFO << "VehicleState updated.";
 
-  Process(point_cloud_msg);
-}
-
-bool DataGenerator::Process(const sensor_msgs::PointCloud2& message) {
-  PointCloudPtr cld(new PointCloud);
-  TransPointCloudMsgToPCL(message, &cld);
-  AINFO << "PointCloud size = " << cld->points.size();
-
-  std::shared_ptr<Matrix4d> velodyne_to_novatel_trans =
-      std::make_shared<Matrix4d>();
-  if (!GetTrans(FLAGS_novatel_frame_name, FLAGS_velodyne64_frame_name,
-                message.header.stamp.toSec(),
-                velodyne_to_novatel_trans.get())) {
-    AERROR << "Fail to transform velodyne64 to novatel at time: "
-           << message.header.stamp.toSec();
-    return false;
-  }
-
-  if (!TransformPointCloudToWorld(velodyne_to_novatel_trans, &cld)) {
-    AERROR << "Fail to transform point cloud to world.";
-    return false;
-  }
-
-  // TODO(All): output labeled obstacle config data
-  for (size_t i = 0; i < cld->points.size(); ++i) {
-    auto& point = cld->points[i];
-    (*data_file_) << point.x << " " << point.y << " " << point.x << " "
-                  << point.intensity << ", ";
-  }
-  (*data_file_) << "\n";
-  return true;
+  // TODO(Liangliang): register more sensors and use factory to manager.
 }
 
 Status DataGenerator::Start() {
@@ -144,80 +109,6 @@ void DataGenerator::Stop() {
   delete data_file_;
 }
 
-void DataGenerator::TransPointCloudMsgToPCL(
-    const sensor_msgs::PointCloud2& cloud_msg, PointCloudPtr* cloud_pcl) {
-  // transform from ros to pcl
-  pcl::PointCloud<pcl_util::PointXYZIT> in_cloud;
-  pcl::fromROSMsg(cloud_msg, in_cloud);
-  // transform from xyzit to xyzi
-  PointCloudPtr& cloud = *cloud_pcl;
-  cloud->header = in_cloud.header;
-  cloud->width = in_cloud.width;
-  cloud->height = in_cloud.height;
-  cloud->is_dense = in_cloud.is_dense;
-  cloud->sensor_origin_ = in_cloud.sensor_origin_;
-  cloud->sensor_orientation_ = in_cloud.sensor_orientation_;
-  cloud->points.resize(in_cloud.points.size());
-  size_t points_num = 0;
-  for (size_t i = 0; i < in_cloud.size(); ++i) {
-    pcl_util::PointXYZIT& pt = in_cloud.points[i];
-    if (!isnan(pt.x) && !isnan(pt.y) && !isnan(pt.z) && !isnan(pt.intensity)) {
-      cloud->points[points_num].x = pt.x;
-      cloud->points[points_num].y = pt.y;
-      cloud->points[points_num].z = pt.z;
-      cloud->points[points_num].intensity = pt.intensity;
-      ++points_num;
-    }
-  }
-  cloud->points.resize(points_num);
-}
-
-bool DataGenerator::GetTrans(const std::string& to_frame,
-                             const std::string& from_frame,
-                             const double query_time, Matrix4d* trans) {
-  CHECK_NOTNULL(trans);
-  ros::Time query_stamp(query_time);
-  const auto& tf2_buffer = AdapterManager::Tf2Buffer();
-  const double kTf2BuffSize = 10 / 1000.0;  // buff size
-  std::string err_msg;
-  if (!tf2_buffer.canTransform(to_frame, from_frame, query_stamp,
-                               ros::Duration(kTf2BuffSize), &err_msg)) {
-    AERROR << "Cannot transform frame from " << from_frame << " to frame "
-           << to_frame << " , err: " << err_msg
-           << ". Frames: " << tf2_buffer.allFramesAsString();
-    return false;
-  }
-
-  geometry_msgs::TransformStamped transform_stamped;
-  try {
-    transform_stamped =
-        tf2_buffer.lookupTransform(to_frame, from_frame, query_stamp);
-  } catch (tf2::TransformException& ex) {
-    AERROR << "Exception: " << ex.what();
-    return false;
-  }
-
-  Affine3d affine_3d;
-  tf::transformMsgToEigen(transform_stamped.transform, affine_3d);
-  *trans = affine_3d.matrix();
-  ADEBUG << from_frame << " to " << to_frame << ",  trans = " << *trans;
-  return true;
-}
-
-bool DataGenerator::TransformPointCloudToWorld(
-    std::shared_ptr<Matrix4d> velodyne_trans, PointCloudPtr* cld) {
-  Affine3d affine_3d_trans(*velodyne_trans);
-  for (size_t i = 0; i < (*cld)->points.size(); ++i) {
-    auto& pt = (*cld)->points[i];
-    PointD point = {pt.x, pt.y, pt.z, 0};
-    PointD point_world = pcl::transformPoint(point, affine_3d_trans);
-    pt.x = point_world.x;
-    pt.y = point_world.y;
-    pt.z = point_world.z;
-  }
-  return true;
-}
-
 }  // namespace data_generator
 }  // namespace perception
 }  // namespace apollo

modules/perception/tool/data_generator/data_generator.h

@@ -24,10 +24,9 @@
 #include <memory>
 #include <string>
 
-#include "sensor_msgs/PointCloud2.h"
-
 #include "Eigen/Core"
 #include "ros/include/ros/ros.h"
+#include "sensor_msgs/PointCloud2.h"
 
 #include "modules/perception/tool/data_generator/proto/config.pb.h"
 
@@ -54,16 +53,8 @@ class DataGenerator : public apollo::common::ApolloApp {
 
  private:
   void RunOnce();
-  bool Process(const sensor_msgs::PointCloud2& message);
-  void OnTimer(const ros::TimerEvent&);
 
-  void TransPointCloudMsgToPCL(const sensor_msgs::PointCloud2& cloud_msg,
-                               pcl_util::PointCloudPtr* cloud_pcl);
-  bool GetTrans(const std::string& to_frame, const std::string& from_frame,
-                const double query_time, Eigen::Matrix4d* trans);
-  bool TransformPointCloudToWorld(
-      std::shared_ptr<Eigen::Matrix4d> velodyne_trans,
-      pcl_util::PointCloudPtr* cld);
+  void OnTimer(const ros::TimerEvent&);
 
   void RegisterSensors();
 

modules/perception/tool/data_generator/sensor.h

@@ -21,6 +21,7 @@
 #ifndef MODULES_PERCEPTION_DATA_GENERATOR_SENSOR_H_
 #define MODULES_PERCEPTION_DATA_GENERATOR_SENSOR_H_
 
+#include <sstream>
 #include <string>
 
 #include "modules/perception/tool/data_generator/proto/config.pb.h"
@@ -40,9 +41,14 @@ class Sensor {
     return config_.sensor_frame_name();
   }
   virtual bool Process() = 0;
+  virtual const std::string& data() const {
+    return data_;
+  }
 
  protected:
   SensorConfig config_;
+  std::stringstream ss_;
+  std::string data_;
 };
 
 }  // namespace data_generator

modules/perception/tool/data_generator/velodyne64.cc

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#include "modules/perception/tool/data_generator/velodyne64.h"
+
+namespace apollo {
+namespace perception {
+namespace data_generator {
+
+using apollo::common::ErrorCode;
+using apollo::common::Status;
+using apollo::common::VehicleState;
+using apollo::common::VehicleStateProvider;
+using apollo::common::adapter::AdapterManager;
+using apollo::perception::pcl_util::PointCloud;
+using apollo::perception::pcl_util::PointCloudPtr;
+using apollo::perception::pcl_util::PointD;
+using apollo::perception::pcl_util::PointXYZIT;
+using Eigen::Affine3d;
+using Eigen::Matrix4d;
+
+bool Velodyne64::Process() {
+  const auto& point_cloud_msg =
+      AdapterManager::GetPointCloud()->GetLatestObserved();
+  ADEBUG << "PointCloud: " << point_cloud_msg.header;
+  ProcessPointCloudData(point_cloud_msg);
+  return true;
+}
+
+bool Velodyne64::ProcessPointCloudData(
+    const sensor_msgs::PointCloud2& message) {
+  PointCloudPtr cld(new PointCloud);
+  TransPointCloudMsgToPCL(message, &cld);
+  AINFO << "PointCloud size = " << cld->points.size();
+
+  std::shared_ptr<Matrix4d> velodyne_to_novatel_trans =
+      std::make_shared<Matrix4d>();
+  if (!GetTrans(FLAGS_novatel_frame_name, FLAGS_velodyne64_frame_name,
+                message.header.stamp.toSec(),
+                velodyne_to_novatel_trans.get())) {
+    AERROR << "Fail to transform velodyne64 to novatel at time: "
+           << message.header.stamp.toSec();
+    return false;
+  }
+
+  if (!TransformPointCloudToWorld(velodyne_to_novatel_trans, &cld)) {
+    AERROR << "Fail to transform point cloud to world.";
+    return false;
+  }
+
+  for (size_t i = 0; i < cld->points.size(); ++i) {
+    auto& point = cld->points[i];
+    ss_ << point.x << " " << point.y << " " << point.x << " " << point.intensity
+        << ", ";
+  }
+  ss_ << "\n";
+  data_ = ss_.str();
+  return true;
+}
+
+void Velodyne64::TransPointCloudMsgToPCL(
+    const sensor_msgs::PointCloud2& cloud_msg, PointCloudPtr* cloud_pcl) {
+  // transform from ros to pcl
+  pcl::PointCloud<pcl_util::PointXYZIT> in_cloud;
+  pcl::fromROSMsg(cloud_msg, in_cloud);
+  // transform from xyzit to xyzi
+  PointCloudPtr& cloud = *cloud_pcl;
+  cloud->header = in_cloud.header;
+  cloud->width = in_cloud.width;
+  cloud->height = in_cloud.height;
+  cloud->is_dense = in_cloud.is_dense;
+  cloud->sensor_origin_ = in_cloud.sensor_origin_;
+  cloud->sensor_orientation_ = in_cloud.sensor_orientation_;
+  cloud->points.resize(in_cloud.points.size());
+  size_t points_num = 0;
+  for (size_t i = 0; i < in_cloud.size(); ++i) {
+    pcl_util::PointXYZIT& pt = in_cloud.points[i];
+    if (!isnan(pt.x) && !isnan(pt.y) && !isnan(pt.z) && !isnan(pt.intensity)) {
+      cloud->points[points_num].x = pt.x;
+      cloud->points[points_num].y = pt.y;
+      cloud->points[points_num].z = pt.z;
+      cloud->points[points_num].intensity = pt.intensity;
+      ++points_num;
+    }
+  }
+  cloud->points.resize(points_num);
+}
+
+bool Velodyne64::GetTrans(const std::string& to_frame,
+                          const std::string& from_frame,
+                          const double query_time, Matrix4d* trans) {
+  CHECK_NOTNULL(trans);
+  ros::Time query_stamp(query_time);
+  const auto& tf2_buffer = AdapterManager::Tf2Buffer();
+  const double kTf2BuffSize = 10 / 1000.0;  // buff size
+  std::string err_msg;
+  if (!tf2_buffer.canTransform(to_frame, from_frame, query_stamp,
+                               ros::Duration(kTf2BuffSize), &err_msg)) {
+    AERROR << "Cannot transform frame from " << from_frame << " to frame "
+           << to_frame << " , err: " << err_msg
+           << ". Frames: " << tf2_buffer.allFramesAsString();
+    return false;
+  }
+
+  geometry_msgs::TransformStamped transform_stamped;
+  try {
+    transform_stamped =
+        tf2_buffer.lookupTransform(to_frame, from_frame, query_stamp);
+  } catch (tf2::TransformException& ex) {
+    AERROR << "Exception: " << ex.what();
+    return false;
+  }
+
+  Affine3d affine_3d;
+  tf::transformMsgToEigen(transform_stamped.transform, affine_3d);
+  *trans = affine_3d.matrix();
+  ADEBUG << from_frame << " to " << to_frame << ",  trans = " << *trans;
+  return true;
+}
+
+bool Velodyne64::TransformPointCloudToWorld(
+    std::shared_ptr<Matrix4d> velodyne_trans, PointCloudPtr* cld) {
+  Affine3d affine_3d_trans(*velodyne_trans);
+  for (size_t i = 0; i < (*cld)->points.size(); ++i) {
+    auto& pt = (*cld)->points[i];
+    PointD point = {pt.x, pt.y, pt.z, 0};
+    PointD point_world = pcl::transformPoint(point, affine_3d_trans);
+    pt.x = point_world.x;
+    pt.y = point_world.y;
+    pt.z = point_world.z;
+  }
+  return true;
+}
+
+}  // namespace data_generator
+}  // namespace perception
+}  // namespace apollo

modules/perception/tool/data_generator/velodyne64.h

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#ifndef MODULES_PERCEPTION_DATA_GENERATOR_VELODYNE64_H_
+#define MODULES_PERCEPTION_DATA_GENERATOR_VELODYNE64_H_
+
+#include <memory>
+#include <string>
+
+#include "Eigen/Core"
+#include "eigen_conversions/eigen_msg.h"
+#include "pcl_conversions/pcl_conversions.h"
+#include "ros/include/ros/ros.h"
+#include "sensor_msgs/PointCloud2.h"
+
+#include "modules/perception/tool/data_generator/proto/config.pb.h"
+
+#include "modules/common/adapters/adapter_manager.h"
+#include "modules/common/log.h"
+#include "modules/common/vehicle_state/vehicle_state_provider.h"
+#include "modules/perception/lib/pcl_util/pcl_types.h"
+#include "modules/perception/tool/data_generator/common/data_generator_gflags.h"
+#include "modules/perception/tool/data_generator/sensor.h"
+
+namespace apollo {
+namespace perception {
+namespace data_generator {
+
+class Velodyne64 : public Sensor {
+ public:
+  explicit Velodyne64(const SensorConfig& config) : Sensor(config) {}
+  virtual ~Velodyne64() = default;
+  bool Process() override;
+
+ private:
+  bool ProcessPointCloudData(const sensor_msgs::PointCloud2& message);
+  void TransPointCloudMsgToPCL(const sensor_msgs::PointCloud2& cloud_msg,
+                               pcl_util::PointCloudPtr* cloud_pcl);
+  bool GetTrans(const std::string& to_frame, const std::string& from_frame,
+                const double query_time, Eigen::Matrix4d* trans);
+  bool TransformPointCloudToWorld(
+      std::shared_ptr<Eigen::Matrix4d> velodyne_trans,
+      pcl_util::PointCloudPtr* cld);
+};
+
+}  // namespace data_generator
+}  // namespace perception
+}  // namespace apollo
+
+#endif  // MODULES_PERCEPTION_DATA_GENERATOR_VELODYNE64_H_