swri_transform_util: local_xy

Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #ifndef TRANSFORM_UTIL_LOCAL_XY_UTIL_H_
00031 #define TRANSFORM_UTIL_LOCAL_XY_UTIL_H_
00032 
00033 #include <string>
00034 
00035 #include <boost/shared_ptr.hpp>
00036 
00037 #include <ros/ros.h>
00038 #include <swri_transform_util/transform_util.h>
00039 #include <topic_tools/shape_shifter.h>
00040 
00041 namespace swri_transform_util
00042 {
00054   void LocalXyFromWgs84(
00055       double latitude,
00056       double longitude,
00057       double reference_latitude,
00058       double reference_longitude,
00059       double& x,
00060       double& y);
00061 
00075   void Wgs84FromLocalXy(
00076       double x,
00077       double y,
00078       double reference_latitude,
00079       double reference_longitude,
00080       double& latitude,
00081       double& longitude);
00082 
00100   class LocalXyWgs84Util
00101   {
00102   public:
00113     LocalXyWgs84Util(
00114         double reference_latitude,
00115         double reference_longitude,
00116         double reference_angle = 0,
00117         double reference_altitude = 0);
00118 
00126     LocalXyWgs84Util();
00127 
00136     bool Initialized() const { return initialized_; }
00137 
00142     void ResetInitialization();
00143 
00149     double ReferenceLongitude() const;
00150 
00156     double ReferenceLatitude() const;
00157 
00161     double ReferenceAngle() const;
00162 
00168     double ReferenceAltitude() const;
00169 
00175     std::string Frame() const { return frame_; }
00176 
00182     std::string NormalizedFrame() const { return NormalizeFrameId(frame_); }
00183 
00194     bool ToLocalXy(
00195         double latitude,
00196         double longitude,
00197         double& x,
00198         double& y) const;
00199 
00210     bool ToWgs84(
00211         double x,
00212         double y,
00213         double& latitude,
00214         double& longitude) const;
00215 
00216   protected:
00217     double reference_latitude_;   //< Reference latitude in radians.
00218     double reference_longitude_;  //< Reference longitude in radians.
00219     double reference_angle_;      //< Reference angle in radians ENU.
00220     double reference_altitude_;   //< Reference altitude in meters.
00221 
00222     double rho_lat_;
00223     double rho_lon_;
00224     double cos_angle_;
00225     double sin_angle_;
00226 
00227     std::string frame_;
00228 
00229     ros::Subscriber origin_sub_;
00230     bool initialized_;
00231 
00232     void Initialize();
00233 
00234     void HandleOrigin(const topic_tools::ShapeShifter::ConstPtr origin);
00235   };
00236   typedef boost::shared_ptr<LocalXyWgs84Util> LocalXyWgs84UtilPtr;
00237 }
00238 
00239 #endif  // TRANSFORM_UTIL_LOCAL_XY_UTIL_H_