00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef pcl_ros_IMPL_TRANSFORMS_H_
00038 #define pcl_ros_IMPL_TRANSFORMS_H_
00039
00040 #include <pcl_conversions/pcl_conversions.h>
00041 #include "pcl_ros/transforms.h"
00042
00043 using pcl_conversions::fromPCL;
00044 using pcl_conversions::toPCL;
00045
00046 namespace pcl_ros
00047 {
00049 template <typename PointT> void
00050 transformPointCloudWithNormals (const pcl::PointCloud <PointT> &cloud_in,
00051 pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
00052 {
00053
00054
00055
00056
00057
00058 tf::Quaternion q = transform.getRotation ();
00059 Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ());
00060 tf::Vector3 v = transform.getOrigin ();
00061 Eigen::Vector3f origin (v.x (), v.y (), v.z ());
00062
00063
00064
00065
00066 transformPointCloudWithNormals (cloud_in, cloud_out, origin, rotation);
00067 }
00068
00070 template <typename PointT> void
00071 transformPointCloud (const pcl::PointCloud <PointT> &cloud_in,
00072 pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform)
00073 {
00074
00075
00076
00077
00078
00079 tf::Quaternion q = transform.getRotation ();
00080 Eigen::Quaternionf rotation (q.w (), q.x (), q.y (), q.z ());
00081 tf::Vector3 v = transform.getOrigin ();
00082 Eigen::Vector3f origin (v.x (), v.y (), v.z ());
00083
00084
00085
00086
00087 transformPointCloud (cloud_in, cloud_out, origin, rotation);
00088 }
00089
00091 template <typename PointT> bool
00092 transformPointCloudWithNormals (const std::string &target_frame,
00093 const pcl::PointCloud <PointT> &cloud_in,
00094 pcl::PointCloud <PointT> &cloud_out,
00095 const tf::TransformListener &tf_listener)
00096 {
00097 if (cloud_in.header.frame_id == target_frame)
00098 {
00099 cloud_out = cloud_in;
00100 return (true);
00101 }
00102
00103 tf::StampedTransform transform;
00104 try
00105 {
00106 tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
00107 }
00108 catch (tf::LookupException &e)
00109 {
00110 ROS_ERROR ("%s", e.what ());
00111 return (false);
00112 }
00113 catch (tf::ExtrapolationException &e)
00114 {
00115 ROS_ERROR ("%s", e.what ());
00116 return (false);
00117 }
00118
00119 transformPointCloudWithNormals (cloud_in, cloud_out, transform);
00120 cloud_out.header.frame_id = target_frame;
00121 return (true);
00122 }
00123
00125 template <typename PointT> bool
00126 transformPointCloud (const std::string &target_frame,
00127 const pcl::PointCloud <PointT> &cloud_in,
00128 pcl::PointCloud <PointT> &cloud_out,
00129 const tf::TransformListener &tf_listener)
00130 {
00131 if (cloud_in.header.frame_id == target_frame)
00132 {
00133 cloud_out = cloud_in;
00134 return (true);
00135 }
00136
00137 tf::StampedTransform transform;
00138 try
00139 {
00140 tf_listener.lookupTransform (target_frame, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, transform);
00141 }
00142 catch (tf::LookupException &e)
00143 {
00144 ROS_ERROR ("%s", e.what ());
00145 return (false);
00146 }
00147 catch (tf::ExtrapolationException &e)
00148 {
00149 ROS_ERROR ("%s", e.what ());
00150 return (false);
00151 }
00152 transformPointCloud (cloud_in, cloud_out, transform);
00153 cloud_out.header.frame_id = target_frame;
00154 return (true);
00155 }
00156
00158 template <typename PointT> bool
00159 transformPointCloud (const std::string &target_frame,
00160 const ros::Time & target_time,
00161 const pcl::PointCloud <PointT> &cloud_in,
00162 const std::string &fixed_frame,
00163 pcl::PointCloud <PointT> &cloud_out,
00164 const tf::TransformListener &tf_listener)
00165 {
00166 tf::StampedTransform transform;
00167 try
00168 {
00169 tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
00170 }
00171 catch (tf::LookupException &e)
00172 {
00173 ROS_ERROR ("%s", e.what ());
00174 return (false);
00175 }
00176 catch (tf::ExtrapolationException &e)
00177 {
00178 ROS_ERROR ("%s", e.what ());
00179 return (false);
00180 }
00181
00182 transformPointCloud (cloud_in, cloud_out, transform);
00183 cloud_out.header.frame_id = target_frame;
00184 std_msgs::Header header;
00185 header.stamp = target_time;
00186 cloud_out.header = toPCL(header);
00187 return (true);
00188 }
00189
00191 template <typename PointT> bool
00192 transformPointCloudWithNormals (const std::string &target_frame,
00193 const ros::Time & target_time,
00194 const pcl::PointCloud <PointT> &cloud_in,
00195 const std::string &fixed_frame,
00196 pcl::PointCloud <PointT> &cloud_out,
00197 const tf::TransformListener &tf_listener)
00198 {
00199 tf::StampedTransform transform;
00200 try
00201 {
00202 tf_listener.lookupTransform (target_frame, target_time, cloud_in.header.frame_id, fromPCL(cloud_in.header).stamp, fixed_frame, transform);
00203 }
00204 catch (tf::LookupException &e)
00205 {
00206 ROS_ERROR ("%s", e.what ());
00207 return (false);
00208 }
00209 catch (tf::ExtrapolationException &e)
00210 {
00211 ROS_ERROR ("%s", e.what ());
00212 return (false);
00213 }
00214
00215 transformPointCloudWithNormals (cloud_in, cloud_out, transform);
00216 cloud_out.header.frame_id = target_frame;
00217 std_msgs::Header header;
00218 header.stamp = target_time;
00219 cloud_out.header = toPCL(header);
00220 return (true);
00221 }
00222
00223 }
00224 #endif