Feature/nmea to mgrs

ros/src/computing/perception/localization/lib/gnss/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(gnss)
 
-find_package(catkin REQUIRED COMPONENTS roscpp)
+find_package(catkin REQUIRED COMPONENTS roscpp geodesy)
 
 catkin_package(
         INCLUDE_DIRS include

ros/src/computing/perception/localization/lib/gnss/include/gnss/geo_pos_conv.hpp

@@ -2,7 +2,8 @@
 #define __GEO_POS_CONV__
 
 #include <math.h>
-
+#include <string>
+#include <geodesy/utm.h>
 class geo_pos_conv {
 private:
 	double m_x;  //m
@@ -12,15 +13,17 @@ class geo_pos_conv {
 	double m_lat;  //latitude
 	double m_lon; //longitude
 	double m_h;
-  
+
 	double m_PLato;        //plane lat
 	double m_PLo;          //plane lon
 
+	std::string m_mgrs_zone; //zone id
+
 public:
 	double x() const;
 	double y() const;
 	double z() const;
-  
+
 	void set_plane(double lat,   double lon);
 	void set_plane(int num);
 	void set_xyz(double cx,   double cy,   double cz);
@@ -29,9 +32,10 @@ class geo_pos_conv {
 	void set_llh(double lat, double lon, double h);
 
 	//set llh in nmea degrees
-	void set_llh_nmea_degrees(double latd,double lond, double h);
+	void set_llh_nmea_degrees(double latd,double lond, double h, bool use_mgrs = false);
 
-        void llh_to_xyz(double lat, double lon, double ele);
+  void llh_to_xyz(double lat, double lon, double ele, bool use_mgrs = false);
+  void conv_llh2mgrs( double latitude_deg, double longitude_deg, double h);
 
 	void conv_llh2xyz(void);
 	void conv_xyz2llh(void);

ros/src/computing/perception/localization/lib/gnss/package.xml

@@ -8,7 +8,9 @@
     <buildtool_depend>catkin</buildtool_depend>
 
     <build_depend>roscpp</build_depend>
+    <build_depend>geodesy</build_depend>
 
     <run_depend>roscpp</run_depend>
+    <run_depend>geodesy</run_depend>
 
 </package>

ros/src/computing/perception/localization/lib/gnss/src/geo_pos_conv.cpp

@@ -187,7 +187,7 @@ void geo_pos_conv::set_xyz(double cx, double cy, double cz)
   conv_xyz2llh();
 }
 
-void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h)
+void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h, bool use_mgrs)
 {
   double lat, lad, lod, lon;
   // 1234.56 -> 12'34.56 -> 12+ 34.56/60
@@ -202,16 +202,89 @@ void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h)
   m_lon = (lod + lon / 60.0) * M_PI / 180;
   m_h = h;
 
-  conv_llh2xyz();
+  if (use_mgrs) //convert from rad -> 0~180 deg -> -90~90 deg
+    conv_llh2mgrs(m_lat/M_PI * 180 - 90, m_lon/M_PI * 180 - 90, h);
+  else
+    conv_llh2xyz();
+
+}
+
+void geo_pos_conv::conv_llh2mgrs( double latitude_deg, double longitude_deg, double h)
+{
+    //you are at north/south pole
+    if(latitude_deg >= 84 || latitude_deg <= -80) {
+        ROS_ERROR_STREAM("Invalid latitude to convert to UTM, but calculating it anyway!");
+    }
+
+    geographic_msgs::GeoPoint wgs_point;
+    wgs_point.latitude = latitude_deg;
+    wgs_point.longitude = longitude_deg;
+    wgs_point.altitude = h;
+
+    geodesy::UTMPoint utm_point;
+    geodesy::fromMsg(wgs_point,utm_point);
+
+    std::string easting_letters = "ABCDEFGHJKLMNPQRSTUVWXYZ";
+    std::string northing_letters = "ABCDEFGHJKLMNPQRSTUV";
+
+    //first letter
+    int group = utm_point.zone % 6;
+    int easting_letter_offset = 0;
+    int northing_letter_offset = 0;
+    switch(group)
+    {
+        case 1:
+            easting_letter_offset = 0;             //A
+            northing_letter_offset = 0;             //A
+            break;
+        case 2:
+            easting_letter_offset = 8;             //J
+            northing_letter_offset = 5;             //F
+            break;
+        case 3:
+            easting_letter_offset = 16;             //S
+            northing_letter_offset = 0;             //A
+            break;
+        case 4:
+            easting_letter_offset = 0;             //A
+            northing_letter_offset = 5;             //F
+            break;
+        case 5:
+            easting_letter_offset = 8;             //J
+            northing_letter_offset = 0;             //A
+            break;
+        case 0:
+            easting_letter_offset = 16;             //S
+            northing_letter_offset = 5;             //F
+            break;
+    }
+
+    int easting_idx = (int)(utm_point.easting/1e5) + easting_letter_offset - 1;     //subtract -1 so that letter starts from A
+    char easting_letter = easting_letters.at( easting_idx );
+
+    int northing_idx = (int)(fmod(utm_point.northing,2e6))/1e5 + northing_letter_offset;
+    northing_idx = northing_idx % northing_letters.size();
+    char northing_letter = northing_letters.at( northing_idx );
+
+    m_x = fmod(utm_point.easting,1e5);
+    m_y = fmod(utm_point.northing,1e5);
+    m_z = h;
+
+    std::stringstream ss;
+    ss << (int)utm_point.zone << utm_point.band << easting_letter << northing_letter;
+    m_mgrs_zone = ss.str();
+
 }
 
-void geo_pos_conv::llh_to_xyz(double lat, double lon, double ele)
+void geo_pos_conv::llh_to_xyz(double lat, double lon, double ele, bool use_mgrs)
 {
   m_lat = lat * M_PI / 180;
   m_lon = lon * M_PI / 180;
   m_h = ele;
-
-  conv_llh2xyz();
+  if(use_mgrs)
+    conv_llh2mgrs(lat, lon, ele);
+  else
+    conv_llh2xyz();
 }
 
 void geo_pos_conv::conv_llh2xyz(void)

ros/src/computing/perception/localization/packages/gnss_localizer/launch/fix2tfpose.launch

@@ -2,9 +2,11 @@
 <launch>
 
   <arg name="plane" default="7"/>
+  <arg name="use_mgrs" default="false"/>
 
   <node pkg="gnss_localizer" type="fix2tfpose" name="fix2tfpose" output="log">
     <param name="plane" value="$(arg plane)"/>
+    <param name="use_mgrs" value="$(arg use_mgrs)"/>
   </node>
 
 </launch>

ros/src/computing/perception/localization/packages/gnss_localizer/launch/nmea2tfpose.launch

@@ -2,9 +2,10 @@
 <launch>
 
   <arg name="plane" default="7"/>
-
+  <arg name="use_mgrs" default="false"/>
   <node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="log">
     <param name="plane" value="$(arg plane)"/>
+    <param name="use_mgrs" value="$(arg use_mgrs)"/>
   </node>
 
 </launch>

ros/src/computing/perception/localization/packages/gnss_localizer/nodes/fix2tfpose/fix2tfpose.cpp

@@ -35,13 +35,15 @@ static geometry_msgs::Quaternion _quat;
 static double yaw;
 
 static int _plane;
+static bool _use_mgrs;
 
 static void GNSSCallback(const sensor_msgs::NavSatFixConstPtr &msg)
 {
   geo_pos_conv geo;
 
   geo.set_plane(_plane);
-  geo.llh_to_xyz(msg->latitude, msg->longitude, msg->altitude);
+
+  geo.llh_to_xyz(msg->latitude, msg->longitude, msg->altitude, _use_mgrs);
 
   static tf::TransformBroadcaster pose_broadcaster;
   tf::Transform pose_transform;
@@ -96,6 +98,7 @@ int main(int argc, char **argv)
   ros::NodeHandle nh;
   ros::NodeHandle private_nh("~");
   private_nh.getParam("plane", _plane);
+  private_nh.getParam("use_mgrs", _use_mgrs);
   pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("gnss_pose", 1000);
   stat_publisher = nh.advertise<std_msgs::Bool>("/gnss_stat", 1000);
   ros::Subscriber gnss_pose_subscriber = nh.subscribe("fix", 100, GNSSCallback);

ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.cpp

@@ -113,7 +113,7 @@ void Nmea2TFPoseNode::convert(std::vector<std::string> nmea, ros::Time current_s
       double lat = stod(nmea.at(2));
       double lon = stod(nmea.at(4));
       double h = stod(nmea.at(9));
-      geo_.set_llh_nmea_degrees(lat, lon, h);
+      geo_.set_llh_nmea_degrees(lat, lon, h, use_mgrs_);
       ROS_INFO("GGA is subscribed.");
     }
     else if(nmea.at(0) == "$GPRMC")
@@ -122,7 +122,7 @@ void Nmea2TFPoseNode::convert(std::vector<std::string> nmea, ros::Time current_s
       double lat = stod(nmea.at(3));
       double lon = stod(nmea.at(5));
       double h = 0.0;
-      geo_.set_llh_nmea_degrees(lat, lon, h);
+      geo_.set_llh_nmea_degrees(lat, lon, h, use_mgrs_);
       ROS_INFO("GPRMC is subscribed.");
     }
   }catch (const std::exception &e)

ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.h

@@ -62,6 +62,7 @@ class Nmea2TFPoseNode
   double orientation_time_, position_time_;
   ros::Time current_time_, orientation_stamp_;
   tf::TransformBroadcaster br_;
+  bool use_mgrs_;
 
   // callbacks
   void callbackFromNmeaSentence(const nmea_msgs::Sentence::ConstPtr &msg);

ros/src/util/packages/runtime_manager/scripts/computing.yaml

@@ -13,12 +13,16 @@ subs :
         param: gnss
         gui  :
           stat_topic : [ gnss ]
+          plane :
+            flags : [ nl ]
       - name : nmea2tfpose
         desc : nmea2tfpose desc sample
         cmd  : roslaunch gnss_localizer nmea2tfpose.launch
         param: gnss
         gui  :
           stat_topic : [ gnss ]
+          plane :
+            flags : [ nl ]
     - name : lidar_localizer
       desc : lidar_localizer desc sample
       subs :
@@ -1435,6 +1439,14 @@ params :
       cmd_param :
         dash        : ''
         delim       : ':='
+    - name    : use_mgrs
+      desc : Use MGRS for xyz coordinate
+      label   : 'use_mgrs'
+      kind    : checkbox
+      v    : False
+      cmd_param:
+        dash     : ''
+        delim    : ':='
 
   - name  : ndt
     topic : /config/ndt