Go to the documentation of this file.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 #ifndef TRANSFORM_UTIL_UTM_TRANSFORMER_H_
00031 #define TRANSFORM_UTIL_UTM_TRANSFORMER_H_
00032
00033 #include <map>
00034 #include <string>
00035 #include <vector>
00036
00037 #include <boost/shared_ptr.hpp>
00038
00039 #include <tf/transform_datatypes.h>
00040 #include <tf/transform_listener.h>
00041
00042 #include <swri_transform_util/utm_util.h>
00043 #include <swri_transform_util/local_xy_util.h>
00044 #include <swri_transform_util/transformer.h>
00045
00046 namespace swri_transform_util
00047 {
00051 class UtmTransformer : public Transformer
00052 {
00053 public:
00054 UtmTransformer();
00055
00062 virtual std::map<std::string, std::vector<std::string> > Supports() const;
00063
00064
00080 virtual bool GetTransform(
00081 const std::string& target_frame,
00082 const std::string& source_frame,
00083 const ros::Time& time,
00084 Transform& transform);
00085
00086 protected:
00087 virtual bool Initialize();
00088
00089 boost::shared_ptr<UtmUtil> utm_util_;
00090 boost::shared_ptr<LocalXyWgs84Util> local_xy_util_;
00091
00092 int32_t utm_zone_;
00093 char utm_band_;
00094 std::string local_xy_frame_;
00095 };
00096
00097
00104 class UtmToTfTransform : public TransformImpl
00105 {
00106 public:
00107 UtmToTfTransform(
00108 const tf::StampedTransform& transform,
00109 boost::shared_ptr<UtmUtil> utm_util,
00110 boost::shared_ptr<LocalXyWgs84Util> local_xy_util,
00111 int32_t utm_zone,
00112 char utm_band);
00113
00114 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00115
00116 virtual tf::Quaternion GetOrientation() const;
00117 virtual TransformImplPtr Inverse() const;
00118
00119 protected:
00120 tf::StampedTransform transform_;
00121 boost::shared_ptr<UtmUtil> utm_util_;
00122 boost::shared_ptr<LocalXyWgs84Util> local_xy_util_;
00123 int32_t utm_zone_;
00124 char utm_band_;
00125 };
00126
00127
00134 class TfToUtmTransform : public TransformImpl
00135 {
00136 public:
00137 TfToUtmTransform(
00138 const tf::StampedTransform& transform,
00139 boost::shared_ptr<UtmUtil> utm_util,
00140 boost::shared_ptr<LocalXyWgs84Util> local_xy_util,
00141 int32_t utm_zone,
00142 char utm_band);
00143
00144 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00145
00146 virtual tf::Quaternion GetOrientation() const;
00147 virtual TransformImplPtr Inverse() const;
00148
00149 protected:
00150 tf::StampedTransform transform_;
00151 boost::shared_ptr<UtmUtil> utm_util_;
00152 boost::shared_ptr<LocalXyWgs84Util> local_xy_util_;
00153 int32_t utm_zone_;
00154 char utm_band_;
00155 };
00156
00157
00164 class UtmToWgs84Transform : public TransformImpl
00165 {
00166 public:
00167 UtmToWgs84Transform(
00168 boost::shared_ptr<UtmUtil> utm_util,
00169 int32_t utm_zone,
00170 char utm_band);
00171
00172 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00173 virtual TransformImplPtr Inverse() const;
00174
00175 protected:
00176 boost::shared_ptr<UtmUtil> utm_util_;
00177 int32_t utm_zone_;
00178 char utm_band_;
00179 };
00180
00187 class Wgs84ToUtmTransform : public TransformImpl
00188 {
00189 public:
00190 explicit Wgs84ToUtmTransform(
00191 boost::shared_ptr<UtmUtil> utm_util,
00192 int32_t utm_zone,
00193 char utm_band);
00194
00195 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00196 virtual TransformImplPtr Inverse() const;
00197
00198 protected:
00199 boost::shared_ptr<UtmUtil> utm_util_;
00200 int32_t utm_zone_;
00201 char utm_band_;
00202 };
00203 }
00204
00205 #endif // TRANSFORM_UTIL_UTM_TRANSFORMER_H_