From 00f663e991ad15f8abf265eef29ecd08d1f5a878 Mon Sep 17 00:00:00 2001 From: Liangliang Zhang Date: Tue, 25 Sep 2018 16:49:50 -0700 Subject: [PATCH] Perception: done with lidar common. (#497) --- .../common/sensor_manager/sensor_manager.cc | 26 +++++++----------- .../common/sensor_manager/sensor_manager.h | 14 +++++----- modules/perception/lidar/common/BUILD | 27 +++++++++++++++++++ modules/perception/lidar/common/lidar_timer.h | 6 ++--- .../transform_wrapper/transform_wrapper.cc | 24 ++++++++--------- .../transform_wrapper/transform_wrapper.h | 6 +++-- modules/planning/BUILD | 6 ++--- 7 files changed, 65 insertions(+), 44 deletions(-) diff --git a/modules/perception/common/sensor_manager/sensor_manager.cc b/modules/perception/common/sensor_manager/sensor_manager.cc index a20f11230e..dce4c714ee 100644 --- a/modules/perception/common/sensor_manager/sensor_manager.cc +++ b/modules/perception/common/sensor_manager/sensor_manager.cc @@ -15,17 +15,16 @@ *****************************************************************************/ #include "modules/perception/common/sensor_manager/sensor_manager.h" -#include - #include #include #include "cybertron/common/log.h" +#include "google/protobuf/text_format.h" + +#include "modules/perception/proto/sensor_meta_schema.pb.h" -// #include "modules/perception/common/io/io_util.h" #include "modules/perception/lib/config_manager/config_manager.h" #include "modules/perception/lib/io/file_util.h" -#include "modules/perception/proto/sensor_meta_schema.pb.h" namespace apollo { namespace perception { @@ -36,9 +35,7 @@ using apollo::perception::base::SensorInfo; using apollo::perception::base::SensorOrientation; using apollo::perception::base::SensorType; -SensorManager::SensorManager() { - CHECK_EQ(this->Init(), true); -} +SensorManager::SensorManager() { CHECK_EQ(this->Init(), true); } bool SensorManager::Init() { std::lock_guard lock(mutex_); @@ -59,7 +56,7 @@ bool SensorManager::Init() { std::string content; if (!lib::FileUtil::GetFileContent(file_path, &content)) { AERROR << "Failed to get SensorManager config path: " - << FLAGS_obs_sensor_meta_path; + << FLAGS_obs_sensor_meta_path; return false; } @@ -95,10 +92,9 @@ bool SensorManager::Init() { return false; } */ - distort_model_map_.insert( - make_pair(sensor_meta_proto.name(), - std::dynamic_pointer_cast( - distort_model))); + distort_model_map_.insert(make_pair( + sensor_meta_proto.name(), + std::dynamic_pointer_cast(distort_model))); undistort_model_map_.insert(make_pair(sensor_meta_proto.name(), distort_model->get_camera_model())); } @@ -173,8 +169,7 @@ bool SensorManager::IsHdLidar(const std::string& name) const { } bool SensorManager::IsHdLidar(const SensorType& type) const { - return type == SensorType::VELODYNE_64 || - type == SensorType::VELODYNE_32 || + return type == SensorType::VELODYNE_64 || type == SensorType::VELODYNE_32 || type == SensorType::VELODYNE_16; } @@ -189,8 +184,7 @@ bool SensorManager::IsLdLidar(const std::string& name) const { } bool SensorManager::IsLdLidar(const SensorType& type) const { - return type == SensorType::LDLIDAR_4 || - type == SensorType::LDLIDAR_1; + return type == SensorType::LDLIDAR_4 || type == SensorType::LDLIDAR_1; } bool SensorManager::IsLidar(const std::string& name) const { diff --git a/modules/perception/common/sensor_manager/sensor_manager.h b/modules/perception/common/sensor_manager/sensor_manager.h index ad70b47458..b382604ad2 100644 --- a/modules/perception/common/sensor_manager/sensor_manager.h +++ b/modules/perception/common/sensor_manager/sensor_manager.h @@ -23,8 +23,8 @@ #include #include "modules/perception/base/camera.h" -#include "modules/perception/base/sensor_meta.h" #include "modules/perception/base/distortion_model.h" +#include "modules/perception/base/sensor_meta.h" #include "modules/perception/common/perception_gflags.h" #include "modules/perception/lib/singleton/singleton.h" @@ -87,12 +87,12 @@ class SensorManager { std::mutex mutex_; bool inited_ = false; - std::unordered_map sensor_info_map_; - std::unordered_map> distort_model_map_; - std::unordered_map> undistort_model_map_; + std::unordered_map + sensor_info_map_; + std::unordered_map> + distort_model_map_; + std::unordered_map> + undistort_model_map_; }; } // namespace common diff --git a/modules/perception/lidar/common/BUILD b/modules/perception/lidar/common/BUILD index 9bfd725165..24f4853e41 100644 --- a/modules/perception/lidar/common/BUILD +++ b/modules/perception/lidar/common/BUILD @@ -2,6 +2,19 @@ load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) +cc_library( + name = "common", + deps = [ + ":cloud_mask", + ":lidar_frame", + ":lidar_log", + ":lidar_object_util", + ":lidar_point_label", + ":lidar_timer", + ":pcl_util", + ], +) + cc_library( name = "cloud_mask", srcs = [ @@ -77,6 +90,20 @@ cc_library( ], ) +cc_library( + name = "lidar_point_label", + hdrs = [ + "lidar_point_label.h", + ], +) + +cc_library( + name = "lidar_timer", + hdrs = [ + "lidar_timer.h", + ], +) + cc_library( name = "pcl_util", hdrs = [ diff --git a/modules/perception/lidar/common/lidar_timer.h b/modules/perception/lidar/common/lidar_timer.h index 379d13b2c1..bfbcad7257 100644 --- a/modules/perception/lidar/common/lidar_timer.h +++ b/modules/perception/lidar/common/lidar_timer.h @@ -13,8 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. *****************************************************************************/ -#ifndef PERCEPITON_LIDAR_COMMON_LIDAR_TIMER_H_ -#define PERCEPITON_LIDAR_COMMON_LIDAR_TIMER_H_ +#ifndef MODULES_PERCEPTION_LIDAR_COMMON_LIDAR_TIMER_H_ +#define MODULES_PERCEPTION_LIDAR_COMMON_LIDAR_TIMER_H_ #include @@ -43,4 +43,4 @@ class Timer { } // namespace perception } // namespace apollo -#endif // PERCEPITON_LIDAR_COMMON_LIDAR_TIMER_H_ +#endif // MODULES_PERCEPTION_LIDAR_COMMON_LIDAR_TIMER_H_ diff --git a/modules/perception/onboard/transform_wrapper/transform_wrapper.cc b/modules/perception/onboard/transform_wrapper/transform_wrapper.cc index 454093bd29..e8ac2562c4 100644 --- a/modules/perception/onboard/transform_wrapper/transform_wrapper.cc +++ b/modules/perception/onboard/transform_wrapper/transform_wrapper.cc @@ -68,7 +68,7 @@ bool TransformCache::QueryTransform(double timestamp, double delt = timestamp - transforms_.back().timestamp; if (delt > max_duration) { AINFO << "ERROR: query timestamp is " << delt - << "s later than cached timestamp"; + << "s later than cached timestamp"; return false; } else if (delt < 0.0) { AINFO << "ERROR: query earlier timestamp than transform cache"; @@ -79,9 +79,8 @@ bool TransformCache::QueryTransform(double timestamp, if (size == 1) { (*transform) = transforms_.back(); transform->timestamp = timestamp; - AINFO << "use transform at " - << std::to_string(transforms_.back().timestamp) << " for " - << std::to_string(timestamp); + AINFO << "use transform at " << std::to_string(transforms_.back().timestamp) + << " for " << std::to_string(timestamp); } else { double ratio = (timestamp - transforms_[size - 2].timestamp) / @@ -101,9 +100,9 @@ bool TransformCache::QueryTransform(double timestamp, transforms_[size - 1].translation.z() * ratio; AINFO << "estimate pose at " << std::to_string(timestamp) - << " from poses at " - << std::to_string(transforms_[size - 2].timestamp) << " and " - << std::to_string(transforms_[size - 1].timestamp); + << " from poses at " + << std::to_string(transforms_[size - 2].timestamp) << " and " + << std::to_string(transforms_[size - 1].timestamp); } return true; } @@ -137,7 +136,7 @@ bool TransformWrapper::GetSensor2worldTrans( Eigen::Affine3d* novatel2world_trans) { if (!inited_) { AERROR << "TransformWrapper not Initialized," - << " unable to call GetSensor2worldTrans."; + << " unable to call GetSensor2worldTrans."; return false; } @@ -180,8 +179,8 @@ bool TransformWrapper::GetSensor2worldTrans( *novatel2world_trans = novatel2world; } AINFO << "Get pose timestamp: " << std::to_string(timestamp) - << ", pose: " << std::endl - << (*sensor2world_trans).matrix(); + << ", pose: " << std::endl + << (*sensor2world_trans).matrix(); return true; } @@ -224,9 +223,8 @@ bool TransformWrapper::QueryTrans(double timestamp, StampedTransform* trans, if (!tf2_buffer_->canTransform(frame_id, child_frame_id, query_time, FLAGS_obs_tf2_buff_size, &err_string)) { AERROR << "Can not find transform. " << std::to_string(timestamp) - << " frame_id: " << frame_id - << " child_frame_id: " << child_frame_id - << " Error info: " << err_string; + << " frame_id: " << frame_id << " child_frame_id: " << child_frame_id + << " Error info: " << err_string; return false; } diff --git a/modules/perception/onboard/transform_wrapper/transform_wrapper.h b/modules/perception/onboard/transform_wrapper/transform_wrapper.h index 49b528c3e9..8585cdc6da 100644 --- a/modules/perception/onboard/transform_wrapper/transform_wrapper.h +++ b/modules/perception/onboard/transform_wrapper/transform_wrapper.h @@ -16,11 +16,13 @@ #ifndef MODULES_PERCEPTION_ONBOARD_TRANSFORM_WRAPPER_H_ #define MODULES_PERCEPTION_ONBOARD_TRANSFORM_WRAPPER_H_ -#include -#include #include +#include #include +#include "Eigen/Core" +#include "Eigen/Dense" + #include "modules/transform/buffer.h" namespace apollo { diff --git a/modules/planning/BUILD b/modules/planning/BUILD index 94f214a044..a635eff49a 100644 --- a/modules/planning/BUILD +++ b/modules/planning/BUILD @@ -35,15 +35,15 @@ cc_test( "//modules/planning:planning_testdata", ], deps = [ - "@gtest//:main", ":planning_component_lib", + "@gtest//:main", ], ) cc_binary( name = "libplanning_component.so", - deps = [":planning_component_lib"], linkshared = True, + deps = [":planning_component_lib"], ) cc_library( @@ -58,6 +58,7 @@ cc_library( "planning_base.h", "std_planning.h", ], + copts = ["-DMODULE_NAME=\\\"planning\\\""], deps = [ "//modules/common", "//modules/common/configs:config_gflags", @@ -79,7 +80,6 @@ cc_library( "//modules/planning/toolkits/deciders", "//modules/prediction/proto:prediction_proto", ], - copts = ["-DMODULE_NAME=\\\"planning\\\""], ) filegroup( -- GitLab