Method gpsHandler

src/mapOptmization.cpp:900–952

    }

    void gpsHandler(const sensor_msgs::NavSatFixConstPtr& gpsMsg)
    {
        if (gpsMsg->status.status != 0 || isnan(gpsMsg->latitude) || isnan(gpsMsg->longitude) || isnan(gpsMsg->altitude)) 
        {
            if(debugMode) cout<<"Bad message, status: "<<gpsMsg->status.status<<endl;
            return;
        }
        // string gpsFrame = "/gps";
        // tf::TransformListener listener;
        // tf::StampedTransform transformStamped_lidar_to_gps;
        // try
        // {
        //     // ERROR] [1639030193.661625830, 1521156822.162099906]: "lidar_link" passed to lookupTransform argument target_frame does not exist
        //     listener.waitForTransform(lidarFrame, mapFrame, gpsMsg->header.stamp, ros::Duration(0.1));
        //     listener.lookupTransform(lidarFrame, mapFrame, gpsMsg->header.stamp, transformStamped_lidar_to_gps);
        // }
        // catch (tf::TransformException &ex) 
        // {
        //     ROS_ERROR("%s",ex.what());
        //     return;
        // }

        float x,y,z;
        x = sin(deg2rad(gpsMsg->latitude - lati0))*rns;
        y = sin(deg2rad(gpsMsg->longitude - longi0))*rew*cos(deg2rad(lati0));
        z = alti0 - gpsMsg->altitude;

        Eigen::Affine3f trans_gps_to_gpsM = pcl::getTransformation(x,y,z,0,0,0);
        Eigen::Affine3f trans_imu_to_map = trans_gps_to_gpsM;
        // Eigen::Affine3f trans_imu_to_map = affine_gps_to_body*trans_gps_to_gpsM*affine_gps_to_body.inverse()*affine_imu_to_body;
        // cout<<"after conversion x y z: "<<trans_imu_to_map(0,3)<<" "<<trans_imu_to_map(1,3)<<" "<<trans_imu_to_map(2,3)<<endl;
        nav_msgs::Odometry odomGPS;
        // notice PoseWithCovariance order: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis; float64[36] covariance
        // different from GTSAM pose covariance order

        odomGPS.pose.covariance[0] = gpsMsg->position_covariance[0];
        odomGPS.pose.covariance[7] = gpsMsg->position_covariance[4];
        odomGPS.pose.covariance[14] = gpsMsg->position_covariance[8];
        odomGPS.pose.pose.position.x = trans_imu_to_map(0,3);
        odomGPS.pose.pose.position.y = trans_imu_to_map(1,3);
        odomGPS.pose.pose.position.z = trans_imu_to_map(2,3);
        odomGPS.pose.pose.orientation.x = 0;
        odomGPS.pose.pose.orientation.y = 0;
        odomGPS.pose.pose.orientation.z = 0;
        odomGPS.pose.pose.orientation.w = 1.0;
        odomGPS.header.stamp = gpsMsg->header.stamp;
        odomGPS.header.frame_id = "/map";
        gpsQueue.push_back(odomGPS);
        gpsTrajPub.publish(odomGPS);

        gps_file<<odomGPS.header.stamp.toSec()<<" "<<odomGPS.pose.pose.position.x<<" "<<odomGPS.pose.pose.position.y<<" "<<odomGPS.pose.pose.position.z<<
            " "<<odomGPS.pose.covariance[0]<<" "<<odomGPS.pose.covariance[7]<<" "<<odomGPS.pose.covariance[14]<<"\n";
    }


    void gtHandler(const nav_msgs::Odometry::ConstPtr& gtMsg)