[success]convert blh to enu

Author: Ramune6110

include/gnss_preprocessing_component.hpp

@@ -21,32 +21,38 @@ using namespace std;
 class GnssPreprocessingCore
 {
     public:
-        //GnssPreprocessingCore();
-        //GnssPreprocessingCore(ros::NodeHandle &n);
-        //GnssPreprocessingCore(double lat, double lon, double hig);
         GnssPreprocessingCore(ros::NodeHandle &n, double lat, double lon, double hig);
         ~GnssPreprocessingCore();
 
         double pi = 3.1415926535898;
         double a = 6378137.0;
         double ONE_F = 298.257223563;
         double E2 = ((1.0/ONE_F)*(2-(1.0/ONE_F)));
-
-        Eigen::Vector3d origin;
     private:
+        // local variable
         ros::NodeHandle nh;
+        double lat0;
+        double lon0;
+        double hig0;
 
         // publisher
+        ros::Publisher gnss_pose_pub;
+        ros::Publisher gnss_path_pub;
 
         // publish data
         nav_msgs::Path gnss_path;
 
         // subscriber
         ros::Subscriber gnss_sub;
 
+        // tf publish
+        tf::TransformBroadcaster odom_to_baselink_broadcaster;
+        geometry_msgs::TransformStamped odom_to_baselink_trans;
+
         // callback function
         void gnssCallback(const sensor_msgs::NavSatFixConstPtr& gnss_msg);
 
+        // convert (lat, lon, alt) to (x, y, z)
         double deg2rad(double deg); // Convert degrees to radians
         double rad2deg(double rad); // Convert radians to degrees
         Eigen::Vector3d blh2ecef(double p_deg, double l_deg, double h);
@@ -55,6 +61,10 @@ class GnssPreprocessingCore
         Eigen::Matrix3d rotx(double theta);
         Eigen::Matrix3d roty(double theta);
         Eigen::Matrix3d rotz(double theta);
+
+        // different version convert (lat, lon, alt) to (x, y, z)
+        int blh2enu(double lat0, double lon0, double lat, double lon, double *x, double *y);
+        double constrain(double val, double min, double max);
 };
 
 /***********************************************************************
@@ -64,20 +74,16 @@ GnssPreprocessingCore::GnssPreprocessingCore(ros::NodeHandle &n, double lat, dou
 {
     std::cout << "GnssPreprocessingCore Start!" << std::endl;
 
-    std::cout << lat << std::endl;
-    std::cout << lon << std::endl;
-    std::cout << hig << std::endl;
-
-    // ROS Node
+    // init local variable
     nh = n;
 
-    origin(0) = lat;
-    origin(1) = lon;
-    origin(2) = hig;
+    lat0 = lat;
+    lon0 = lon;
+    hig0 = hig;
 
     // Publisher
-    //gnss_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/gnss_pose", 10);
-    //gnss_path_pub = nh.advertise<nav_msgs::Path>("/gnss_path", 10);
+    gnss_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/gnss_pose", 10);
+    gnss_path_pub = nh.advertise<nav_msgs::Path>("/gnss_path", 10);
 
     // Subscriber
     gnss_sub = nh.subscribe("/fix", 10, &GnssPreprocessingCore::gnssCallback, this);
@@ -95,93 +101,96 @@ GnssPreprocessingCore::~GnssPreprocessingCore()
 
 void GnssPreprocessingCore::gnssCallback(const sensor_msgs::NavSatFixConstPtr& gnss_msg)
 {
-    /*
-    std::cout << gnss_msg->latitude << std::endl;
-    std::cout << gnss_msg->longitude << std::endl;
-    std::cout << gnss_msg->altitude << std::endl;
-    */
-
-    /*
-    std::cout << origin(0) << std::endl;
-    std::cout << origin(1) << std::endl;
-    std::cout << origin(2) << std::endl;
-    */
-
     // Convert lat/lon/height to ECEF
-    Eigen::Vector3d ecef_origin = blh2ecef(origin(0), origin(1), origin(2));
-    /*
-    std::cout << ecef_origin(0) << std::endl;
-    std::cout << ecef_origin(1) << std::endl;
-    std::cout << ecef_origin(2) << std::endl;
-    */
+    /***********************************************************************
+    NED座標系になっている可能性がある -> enu
+    enu[0] = N
+    enu[1] = E
+    enu[2] = D
+    となっている可能性があるので、publishする際は現状
+    X = E = enu[1]
+    Y = N = enu[0]
+    としている。
+    **********************************************************************/
+    Eigen::Vector3d ecef_origin = blh2ecef(lat0, lon0, hig0);
     Eigen::Vector3d ecef = blh2ecef(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude);
-    
     Eigen::Vector3d enu = ecef2enu(ecef, ecef_origin);
-    /*
-    std::cout << enu(0) << std::endl;
-    std::cout << enu(1) << std::endl;
-    std::cout << enu(2) << std::endl;
-    */
 
+    /***********************************************************************
+    こちらの処理の場合は正常にENU座標値が算出される
+    enu[0] = E
+    enu[1] = N
+    enu[2] = U
+    publishする際は
+    X = E = enu[0]
+    Y = N = enu[1]
+    とする。
+    **********************************************************************/
     /*
-    data_conversion_gps(gps_msg, gps_data);
-
+    double x, y;
+    blh2enu(lat0, lon0, gnss_msg->latitude, gnss_msg->longitude, &x, &y);
+    Eigen::Vector3d enu = Eigen::Vector3d(x, y, gnss_msg->altitude);
+    */
+    
     geometry_msgs::PoseStamped point;
     point.header.frame_id = "map";
     point.header.stamp = ros::Time::now();
-    point.pose.position.x = gps_data.ned[0];
-    point.pose.position.y = gps_data.ned[1];
+    point.pose.position.x = enu[1];
+    point.pose.position.y = enu[0];
     point.pose.position.z = 0.0;
     point.pose.orientation.w = 0;
     point.pose.orientation.x = 0;
     point.pose.orientation.y = 0;
     point.pose.orientation.z = 1.0;
-    gps_path.poses.push_back(point);
-    gps_pose_pub.publish(point);
-    gps_path_pub.publish(gps_path);
+    
+    gnss_path.poses.push_back(point);
+    gnss_pose_pub.publish(point);
+    gnss_path_pub.publish(gnss_path);
 
     // /odom to /base_link transform broadcast
     odom_to_baselink_trans.header.stamp = ros::Time::now();
     odom_to_baselink_trans.header.frame_id = "odom";
     odom_to_baselink_trans.child_frame_id = "base_link";
-    odom_to_baselink_trans.transform.translation.x = gps_data.ned[0];
-    odom_to_baselink_trans.transform.translation.y = gps_data.ned[1];
+    odom_to_baselink_trans.transform.translation.x = enu[1];
+    odom_to_baselink_trans.transform.translation.y = enu[0];
     odom_to_baselink_trans.transform.translation.z = 0.0;
     odom_to_baselink_trans.transform.rotation.x = 0.0;
     odom_to_baselink_trans.transform.rotation.y = 0.0;
     odom_to_baselink_trans.transform.rotation.z = 0.0;
     odom_to_baselink_trans.transform.rotation.w = 1.0;
     odom_to_baselink_broadcaster.sendTransform(odom_to_baselink_trans);
-    */
 }
 
+/***********************************************************************
+ * Convert (lat, lon, alt) -> (x, y, z) 
+ **********************************************************************/
 Eigen::Vector3d GnssPreprocessingCore::blh2ecef(double p_deg, double l_deg, double h) {
     double p_rad = deg2rad(p_deg);
     double l_rad = deg2rad(l_deg);
 
-    double NN = (a / sqrt(1.0 - (E2)*pow(sin(p_rad),2)));
+    double N = (a / sqrt(1.0 - E2 * pow(sin(p_rad), 2)));
 
-    double x = (NN+h)*cos(p_rad)*cos(l_rad);
-    double y = (NN+h)*cos(p_rad)*sin(l_rad);
-    double z = (NN*(1-E2)+h)*sin(p_rad);
+    double x = (N + h) * cos(p_rad) * cos(l_rad);
+    double y = (N + h) * cos(p_rad) * sin(l_rad);
+    double z = (N * (1 - E2) + h) * sin(p_rad);
 
     return Eigen::Vector3d(x, y, z);
 }
 
 Eigen::Vector3d GnssPreprocessingCore::ecef2blh(double x, double y, double z) {
-    double b = (a*(1.0 - 1.0/ONE_F));
-    double ED2 = (E2*pow(a,2)/(pow(b,2)));
-    double p = sqrt(pow(x,2) + pow(y,2));
-    double si = atan2(z*a, p*b);
+    double b = (a * (1.0 - 1.0 / ONE_F));
+    double ED2 = (E2 * pow(a, 2) / (pow(b, 2)));
+    double p = sqrt(pow(x, 2) + pow(y, 2));
+    double si = atan2(z * a, p * b);
 
-    double p_rad2 = atan2((z + ED2*b*pow(sin(si),3)),(p-E2*a*pow(cos(si),3)));
+    double p_rad2 = atan2((z + ED2 * b * pow(sin(si), 3)), (p - E2 * a * pow(cos(si), 3)));
     double l_rad2 = atan2(y, x);
 
     double lat = rad2deg(p_rad2);
     double lon = rad2deg(l_rad2);
 
-    double NN = (a / sqrt(1.0 - (E2)*pow(sin(p_rad2),2)));
-    double hig = (p/cos(p_rad2)) - NN;
+    double N = (a / sqrt(1.0 - E2 * pow(sin(p_rad2), 2)));
+    double hig = (p / cos(p_rad2)) - N;
 
     return Eigen::Vector3d(lat, lon, hig);
 }
@@ -190,14 +199,15 @@ Eigen::Vector3d GnssPreprocessingCore::ecef2enu(Eigen::Vector3d dest, Eigen::Vec
     Eigen::Vector3d blh = ecef2blh(origin(0), origin(1), origin(2));
 
     Eigen::Matrix3d rotzp1 = rotz(90.0);
-    Eigen::Matrix3d rotyp = roty(90.0-blh(0));
+    Eigen::Matrix3d rotyp = roty(90.0 - blh(0));
     Eigen::Matrix3d rotzp2 = rotz(blh(1));
 
-    Eigen::Matrix3d mat_conv1 = rotzp1*rotyp;
-    Eigen::Matrix3d mat_conv2 = mat_conv1*rotzp2;
+    Eigen::Matrix3d mat_conv1 = rotzp1 * rotyp;
+    Eigen::Matrix3d mat_conv2 = mat_conv1 * rotzp2;
 
     Eigen::Vector3d mov = dest - origin;
-    return mat_conv2*mov;
+
+    return mat_conv2 * mov;
 }
 
 Eigen::Matrix3d GnssPreprocessingCore::rotx(double theta) {
@@ -240,4 +250,49 @@ double GnssPreprocessingCore::rad2deg(double rad) {
     return rad * 180.0 / pi;
 }
 
+/***********************************************************************
+ * Convert (lat, lon, alt) -> (x, y, z) 
+ **********************************************************************/
+int GnssPreprocessingCore::blh2enu(double lat0, double lon0, double lat, double lon, double *x, double *y)
+{
+    static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; //[m]
+
+    const double lat0_rad = lat0 * (M_PI / 180.0);
+	const double lon0_rad = lon0 * (M_PI / 180.0);
+    const double lat_rad = lat * (M_PI / 180.0);
+	const double lon_rad = lon * (M_PI / 180.0);
+
+    const double sin_lat0 = sin(lat0_rad);
+	const double cos_lat0 = cos(lat0_rad);
+	const double sin_lat = sin(lat_rad);
+	const double cos_lat = cos(lat_rad);
+
+	const double cos_d_lon = cos(lon_rad - lon0_rad);
+
+	const double arg = constrain(sin_lat0 * sin_lat + cos_lat0 * cos_lat * cos_d_lon, -1.0,  1.0);
+	const double c = acos(arg);
+
+	double k = 1.0;
+
+	if (fabs(c) > 0) {
+		k = (c / sin(c));
+	}
+
+	*x = static_cast<double>(k * (cos_lat0 * sin_lat - sin_lat0 * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
+	*y = static_cast<double>(k * cos_lat * sin(lon_rad - lon0_rad) * CONSTANTS_RADIUS_OF_EARTH);
+
+	return 0;
+}
+
+double GnssPreprocessingCore::constrain(double val, double min, double max)
+{
+    if (val > max) {
+        return max;
+    } else if (val < min) {
+        return min;
+    } else {
+        return val;
+    }
+}
+
 #endif  // __GNSS_PREPROCESSING_CORE_HPP__

include/gnss_preprocessing_core.hpp

@@ -9,5 +9,5 @@
     <!--rosbag play-->
     <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/ramune/Downloads/utbm_robocar_dataset_20190131_noimage.bag"/>
     <!--display rviz-->
-    <!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find gnss_imu_odom_ESKF)/rviz/gps_trajectory_plotter.rviz" required="true"/>-->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find gnss_preprocessing)/rviz/gnss_preprocessing.rviz" required="true"/>
 </launch>