63 {
64 if (msg->height() == 0 || msg->width() == 0) {
65 AERROR <<
"PointCloud width & height should not be 0";
66 return false;
67 }
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, ×tamp_min, ×tamp_max);
76
77 msg_compensated->mutable_header()->set_timestamp_sec(
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
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();
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.
static Time Now()
get the current time.
bool MotionCompensation(const std::shared_ptr< const PointCloud > &msg, std::shared_ptr< PointCloud > msg_compensated)
std::string frame_id
Point cloud frame id