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 <topic_tools/shape_shifter.h>
00039 
00040 namespace swri_transform_util
00041 {
00053   void LocalXyFromWgs84(
00054       double latitude,
00055       double longitude,
00056       double reference_latitude,
00057       double reference_longitude,
00058       double& x,
00059       double& y);
00060 
00074   void Wgs84FromLocalXy(
00075       double x,
00076       double y,
00077       double reference_latitude,
00078       double reference_longitude,
00079       double& latitude,
00080       double& longitude);
00081 
00099   class LocalXyWgs84Util
00100   {
00101   public:
00112     LocalXyWgs84Util(
00113         double reference_latitude,
00114         double reference_longitude,
00115         double reference_angle = 0,
00116         double reference_altitude = 0);
00117 
00125     LocalXyWgs84Util();
00126 
00135     bool Initialized() const { return initialized_; }
00136 
00142     double ReferenceLongitude() const;
00143 
00149     double ReferenceLatitude() const;
00150 
00154     double ReferenceAngle() const;
00155 
00161     double ReferenceAltitude() const;
00162 
00168     std::string Frame() const { return frame_; }
00169 
00180     bool ToLocalXy(
00181         double latitude,
00182         double longitude,
00183         double& x,
00184         double& y) const;
00185 
00196     bool ToWgs84(
00197         double x,
00198         double y,
00199         double& latitude,
00200         double& longitude) const;
00201 
00202   protected:
00203     double reference_latitude_;   //< Reference latitude in radians.
00204     double reference_longitude_;  //< Reference longitude in radians.
00205     double reference_angle_;      //< Reference angle in radians ENU.
00206     double reference_altitude_;   //< Reference altitude in meters.
00207 
00208     double rho_lat_;
00209     double rho_lon_;
00210     double cos_angle_;
00211     double sin_angle_;
00212 
00213     std::string frame_;
00214 
00215     ros::Subscriber origin_sub_;
00216     bool initialized_;
00217 
00218     void Initialize();
00219 
00220     void HandleOrigin(const topic_tools::ShapeShifter::ConstPtr origin);
00221   };
00222   typedef boost::shared_ptr<LocalXyWgs84Util> LocalXyWgs84UtilPtr;
00223 }
00224 
00225 #endif  // TRANSFORM_UTIL_LOCAL_XY_UTIL_H_