From 582a082d6c6de7e32301361828a455b5679e5f7d Mon Sep 17 00:00:00 2001 From: Liangliang Zhang Date: Thu, 1 Feb 2018 19:48:48 -0800 Subject: [PATCH] DataGenerator: refactored data generator so that we can easily register and manager sensors. --- modules/perception/tool/data_generator/BUILD | 11 +- .../tool/data_generator/data_generator.cc | 113 +------------ .../tool/data_generator/data_generator.h | 13 +- .../perception/tool/data_generator/sensor.h | 6 + .../tool/data_generator/velodyne64.cc | 154 ++++++++++++++++++ .../tool/data_generator/velodyne64.h | 67 ++++++++ 6 files changed, 240 insertions(+), 124 deletions(-) create mode 100644 modules/perception/tool/data_generator/velodyne64.cc create mode 100644 modules/perception/tool/data_generator/velodyne64.h diff --git a/modules/perception/tool/data_generator/BUILD b/modules/perception/tool/data_generator/BUILD index 33b92c00ed..cb1630d54e 100644 --- a/modules/perception/tool/data_generator/BUILD +++ b/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", ], diff --git a/modules/perception/tool/data_generator/data_generator.cc b/modules/perception/tool/data_generator/data_generator.cc index e4f1008a3f..06b72c8f8c 100644 --- a/modules/perception/tool/data_generator/data_generator.cc +++ b/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 velodyne_to_novatel_trans = - std::make_shared(); - 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 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 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 diff --git a/modules/perception/tool/data_generator/data_generator.h b/modules/perception/tool/data_generator/data_generator.h index 7bc25dc5b3..416a383726 100644 --- a/modules/perception/tool/data_generator/data_generator.h +++ b/modules/perception/tool/data_generator/data_generator.h @@ -24,10 +24,9 @@ #include #include -#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 velodyne_trans, - pcl_util::PointCloudPtr* cld); + void OnTimer(const ros::TimerEvent&); void RegisterSensors(); diff --git a/modules/perception/tool/data_generator/sensor.h b/modules/perception/tool/data_generator/sensor.h index 326feb3020..bfbef20f53 100644 --- a/modules/perception/tool/data_generator/sensor.h +++ b/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 #include #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 diff --git a/modules/perception/tool/data_generator/velodyne64.cc b/modules/perception/tool/data_generator/velodyne64.cc new file mode 100644 index 0000000000..2c950a735c --- /dev/null +++ b/modules/perception/tool/data_generator/velodyne64.cc @@ -0,0 +1,154 @@ +/****************************************************************************** + * 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 velodyne_to_novatel_trans = + std::make_shared(); + 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 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 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 diff --git a/modules/perception/tool/data_generator/velodyne64.h b/modules/perception/tool/data_generator/velodyne64.h new file mode 100644 index 0000000000..7b37cb725b --- /dev/null +++ b/modules/perception/tool/data_generator/velodyne64.h @@ -0,0 +1,67 @@ +/****************************************************************************** + * 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 +#include + +#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 velodyne_trans, + pcl_util::PointCloudPtr* cld); +}; + +} // namespace data_generator +} // namespace perception +} // namespace apollo + +#endif // MODULES_PERCEPTION_DATA_GENERATOR_VELODYNE64_H_ -- GitLab