geometry_msgs::Vector3 invert_quat_rotate(geometry_msgs::Vector3 const &point,
geometry_msgs::Quaternion const &quat) {
tf2::Vector3 const tf2_point(point.x, point.y, point.z);
tf2::Quaternion const tf2_quat(quat.x, quat.y, quat.z, quat.w);
tf2::Vector3 const tf2_output = tf2::quatRotate(tf2_quat.inverse(), tf2_point);
geometry_msgs::Vector3 output;
output.x = tf2_output.getX();
output.y = tf2_output.getY();
output.z = tf2_output.getZ();
return output;
}
void LaserScanToROSPointcloud::addMeasureToPointCloud(const tf2::Vector3& point, float intensity) {
*pointcloud_data_position_++ = (float)point.getX();
*pointcloud_data_position_++ = (float)point.getY();
*pointcloud_data_position_++ = (float)point.getZ();
if (include_laser_intensity_) {
*pointcloud_data_position_++ = intensity;
}
}