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
00091 int32_t utm_zone_;
00092 char utm_band_;
00093 std::string local_xy_frame_;
00094 };
00095
00096
00103 class UtmToTfTransform : public TransformImpl
00104 {
00105 public:
00106 UtmToTfTransform(
00107 const tf::StampedTransform& transform,
00108 boost::shared_ptr<UtmUtil> utm_util,
00109 boost::shared_ptr<LocalXyWgs84Util> local_xy_util,
00110 int32_t utm_zone,
00111 char utm_band);
00112
00113 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00114
00115 virtual tf::Quaternion GetOrientation() const;
00116 virtual TransformImplPtr Inverse() const;
00117
00118 protected:
00119 tf::StampedTransform transform_;
00120 boost::shared_ptr<UtmUtil> utm_util_;
00121 boost::shared_ptr<LocalXyWgs84Util> local_xy_util_;
00122 int32_t utm_zone_;
00123 char utm_band_;
00124 };
00125
00126
00133 class TfToUtmTransform : public TransformImpl
00134 {
00135 public:
00136 TfToUtmTransform(
00137 const tf::StampedTransform& transform,
00138 boost::shared_ptr<UtmUtil> utm_util,
00139 boost::shared_ptr<LocalXyWgs84Util> local_xy_util,
00140 int32_t utm_zone,
00141 char utm_band);
00142
00143 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00144
00145 virtual tf::Quaternion GetOrientation() const;
00146 virtual TransformImplPtr Inverse() const;
00147
00148 protected:
00149 tf::StampedTransform transform_;
00150 boost::shared_ptr<UtmUtil> utm_util_;
00151 boost::shared_ptr<LocalXyWgs84Util> local_xy_util_;
00152 int32_t utm_zone_;
00153 char utm_band_;
00154 };
00155
00156
00163 class UtmToWgs84Transform : public TransformImpl
00164 {
00165 public:
00166 UtmToWgs84Transform(
00167 boost::shared_ptr<UtmUtil> utm_util,
00168 int32_t utm_zone,
00169 char utm_band);
00170
00171 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00172 virtual TransformImplPtr Inverse() const;
00173
00174 protected:
00175 boost::shared_ptr<UtmUtil> utm_util_;
00176 int32_t utm_zone_;
00177 char utm_band_;
00178 };
00179
00186 class Wgs84ToUtmTransform : public TransformImpl
00187 {
00188 public:
00189 explicit Wgs84ToUtmTransform(
00190 boost::shared_ptr<UtmUtil> utm_util,
00191 int32_t utm_zone,
00192 char utm_band);
00193
00194 virtual void Transform(const tf::Vector3& v_in, tf::Vector3& v_out) const;
00195 virtual TransformImplPtr Inverse() const;
00196
00197 protected:
00198 boost::shared_ptr<UtmUtil> utm_util_;
00199 int32_t utm_zone_;
00200 char utm_band_;
00201 };
00202 }
00203
00204 #endif // TRANSFORM_UTIL_UTM_TRANSFORMER_H_