70 {
71 if (msg->height() == 0 || msg->width() == 0) {
72 AERROR <<
"PointCloud width & height should not be 0";
73 return false;
74 }
76 Eigen::Affine3d pose_min_time;
77 Eigen::Affine3d pose_max_time;
78
79 uint64_t timestamp_min = 0;
80 uint64_t timestamp_max = 0;
81 std::string
frame_id = msg->header().frame_id();
82 GetTimestampInterval(msg, ×tamp_min, ×tamp_max);
83
84 msg_compensated->mutable_header()->set_timestamp_sec(
86 msg_compensated->mutable_header()->set_frame_id(msg->header().frame_id());
87 msg_compensated->mutable_header()->set_lidar_timestamp(
88 msg->header().lidar_timestamp());
89 msg_compensated->set_measurement_time(msg->measurement_time());
90 msg_compensated->set_height(msg->height());
91 msg_compensated->set_is_dense(msg->is_dense());
92
93 uint64_t new_time = cyber::Time().Now().ToNanosecond();
94 AINFO <<
"compenstator new msg diff:" << new_time - start
95 << ";meta:" << msg->header().lidar_timestamp();
96 msg_compensated->mutable_point()->Reserve(240000);
97
98
99 if (QueryPoseAffineFromTF2(timestamp_min, &pose_min_time, frame_id)
100 && QueryPoseAffineFromTF2(timestamp_max, &pose_max_time, frame_id)) {
101 uint64_t tf_time = cyber::Time().Now().ToNanosecond();
102 AINFO <<
"compenstator tf msg diff:" << tf_time - new_time
103 << ";meta:" << msg->header().lidar_timestamp();
105 msg,
106 msg_compensated,
107 timestamp_min,
108 timestamp_max,
109 pose_min_time,
110 pose_max_time);
111 uint64_t com_time = cyber::Time().Now().ToNanosecond();
112 msg_compensated->set_width(
113 msg_compensated->point_size() / msg->height());
114 AINFO <<
"compenstator com msg diff:" << com_time - tf_time
115 << ";meta:" << msg->header().lidar_timestamp();
116 return true;
117 }
118 return false;
119}
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