Exemplo n.º 1
0
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;
	}
}