diff --git a/ros/src/computing/perception/localization/lib/gnss/CMakeLists.txt b/ros/src/computing/perception/localization/lib/gnss/CMakeLists.txt index 1b7609926e4..442954870ab 100644 --- a/ros/src/computing/perception/localization/lib/gnss/CMakeLists.txt +++ b/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 diff --git a/ros/src/computing/perception/localization/lib/gnss/include/gnss/geo_pos_conv.hpp b/ros/src/computing/perception/localization/lib/gnss/include/gnss/geo_pos_conv.hpp index 0a7943e7dfe..5447785375e 100644 --- a/ros/src/computing/perception/localization/lib/gnss/include/gnss/geo_pos_conv.hpp +++ b/ros/src/computing/perception/localization/lib/gnss/include/gnss/geo_pos_conv.hpp @@ -2,7 +2,8 @@ #define __GEO_POS_CONV__ #include - +#include +#include 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); diff --git a/ros/src/computing/perception/localization/lib/gnss/package.xml b/ros/src/computing/perception/localization/lib/gnss/package.xml index b6d587e628b..150832e5aeb 100644 --- a/ros/src/computing/perception/localization/lib/gnss/package.xml +++ b/ros/src/computing/perception/localization/lib/gnss/package.xml @@ -8,7 +8,9 @@ catkin roscpp + geodesy roscpp + geodesy diff --git a/ros/src/computing/perception/localization/lib/gnss/src/geo_pos_conv.cpp b/ros/src/computing/perception/localization/lib/gnss/src/geo_pos_conv.cpp index 828ed9656f7..5e0925485e8 100644 --- a/ros/src/computing/perception/localization/lib/gnss/src/geo_pos_conv.cpp +++ b/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) diff --git a/ros/src/computing/perception/localization/packages/gnss_localizer/launch/fix2tfpose.launch b/ros/src/computing/perception/localization/packages/gnss_localizer/launch/fix2tfpose.launch index 34a591ae6aa..163c65c5bc8 100644 --- a/ros/src/computing/perception/localization/packages/gnss_localizer/launch/fix2tfpose.launch +++ b/ros/src/computing/perception/localization/packages/gnss_localizer/launch/fix2tfpose.launch @@ -2,9 +2,11 @@ + + diff --git a/ros/src/computing/perception/localization/packages/gnss_localizer/launch/nmea2tfpose.launch b/ros/src/computing/perception/localization/packages/gnss_localizer/launch/nmea2tfpose.launch index 66ae131e52e..c305cc5d26b 100644 --- a/ros/src/computing/perception/localization/packages/gnss_localizer/launch/nmea2tfpose.launch +++ b/ros/src/computing/perception/localization/packages/gnss_localizer/launch/nmea2tfpose.launch @@ -2,9 +2,10 @@ - + + diff --git a/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/fix2tfpose/fix2tfpose.cpp b/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/fix2tfpose/fix2tfpose.cpp index 7faba208ef5..236718bcbe8 100644 --- a/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/fix2tfpose/fix2tfpose.cpp +++ b/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("gnss_pose", 1000); stat_publisher = nh.advertise("/gnss_stat", 1000); ros::Subscriber gnss_pose_subscriber = nh.subscribe("fix", 100, GNSSCallback); diff --git a/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.cpp b/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.cpp index d30bc11ca43..1bff730e074 100644 --- a/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.cpp +++ b/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.cpp @@ -113,7 +113,7 @@ void Nmea2TFPoseNode::convert(std::vector 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 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) diff --git a/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.h b/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.h index 9c3d203ddd4..82f47ac2fa4 100644 --- a/ros/src/computing/perception/localization/packages/gnss_localizer/nodes/nmea2tfpose/nmea2tfpose_core.h +++ b/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); diff --git a/ros/src/util/packages/runtime_manager/scripts/computing.yaml b/ros/src/util/packages/runtime_manager/scripts/computing.yaml index d06b0d3c980..254f60c9e2a 100755 --- a/ros/src/util/packages/runtime_manager/scripts/computing.yaml +++ b/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