Apollo 10.0
自动驾驶开放平台
apollo::drivers::velodyne::Compensator类 参考

#include <compensator.h>

apollo::drivers::velodyne::Compensator 的协作图:

Public 成员函数

 Compensator (const CompensatorConfig &config)
 
virtual ~Compensator ()
 
bool MotionCompensation (const std::shared_ptr< const PointCloud > &msg, std::shared_ptr< PointCloud > msg_compensated)
 

详细描述

在文件 compensator.h41 行定义.

构造及析构函数说明

◆ Compensator()

apollo::drivers::velodyne::Compensator::Compensator ( const CompensatorConfig config)
inlineexplicit

在文件 compensator.h43 行定义.

43: config_(config) {}

◆ ~Compensator()

virtual apollo::drivers::velodyne::Compensator::~Compensator ( )
inlinevirtual

在文件 compensator.h44 行定义.

44{}

成员函数说明

◆ MotionCompensation()

bool apollo::drivers::velodyne::Compensator::MotionCompensation ( const std::shared_ptr< const PointCloud > &  msg,
std::shared_ptr< PointCloud msg_compensated 
)

在文件 compensator.cc61 行定义.

63 {
64 if (msg->height() == 0 || msg->width() == 0) {
65 AERROR << "PointCloud width & height should not be 0";
66 return false;
67 }
68 uint64_t start = cyber::Time::Now().ToNanosecond();
69 Eigen::Affine3d pose_min_time;
70 Eigen::Affine3d pose_max_time;
71
72 uint64_t timestamp_min = 0;
73 uint64_t timestamp_max = 0;
74 std::string frame_id = msg->header().frame_id();
75 GetTimestampInterval(msg, &timestamp_min, &timestamp_max);
76
77 msg_compensated->mutable_header()->set_timestamp_sec(
78 cyber::Time::Now().ToSecond());
79 msg_compensated->mutable_header()->set_frame_id(msg->header().frame_id());
80 msg_compensated->mutable_header()->set_lidar_timestamp(
81 msg->header().lidar_timestamp());
82 msg_compensated->set_measurement_time(msg->measurement_time());
83 msg_compensated->set_height(msg->height());
84 msg_compensated->set_is_dense(msg->is_dense());
85
86 uint64_t new_time = cyber::Time().Now().ToNanosecond();
87 AINFO << "compenstator new msg diff:" << new_time - start
88 << ";meta:" << msg->header().lidar_timestamp();
89 msg_compensated->mutable_point()->Reserve(240000);
90
91 // compensate point cloud, remove nan point
92 if (QueryPoseAffineFromTF2(timestamp_min, &pose_min_time, frame_id) &&
93 QueryPoseAffineFromTF2(timestamp_max, &pose_max_time, frame_id)) {
94 uint64_t tf_time = cyber::Time().Now().ToNanosecond();
95 AINFO << "compenstator tf msg diff:" << tf_time - new_time
96 << ";meta:" << msg->header().lidar_timestamp();
97 MotionCompensation(msg, msg_compensated, timestamp_min, timestamp_max,
98 pose_min_time, pose_max_time);
99 uint64_t com_time = cyber::Time().Now().ToNanosecond();
100 msg_compensated->set_width(msg_compensated->point_size() / msg->height());
101 AINFO << "compenstator com msg diff:" << com_time - tf_time
102 << ";meta:" << msg->header().lidar_timestamp();
103 return true;
104 }
105 return false;
106}
uint64_t ToNanosecond() const
convert time to nanosecond.
Definition time.cc:83
static Time Now()
get the current time.
Definition time.cc:57
bool MotionCompensation(const std::shared_ptr< const PointCloud > &msg, std::shared_ptr< PointCloud > msg_compensated)
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
std::string frame_id
Point cloud frame id

该类的文档由以下文件生成: