38 {
39 if (sp_pose_collection_ == nullptr) {
40 sp_pose_collection_ = std::make_shared<PoseCollection>(sp_conf_);
41 }
42
44 bestgnsspos->measurement_time());
45 FramePose pose;
46 if (sp_conf_->use_system_time) {
48 AINFO <<
"system time: " << pose.time_stamp;
49 } else {
50 pose.time_stamp = time_stamp;
51 }
52 pose.latitude = bestgnsspos->latitude();
53 pose.longitude = bestgnsspos->longitude();
54 pose.altitude = bestgnsspos->height_msl();
55 pose.solution_status = bestgnsspos->sol_status();
56 pose.position_type = bestgnsspos->sol_type();
57 pose.diff_age = bestgnsspos->differential_age();
58 double latitude_std = bestgnsspos->latitude_std_dev();
59 double longitude_std = bestgnsspos->longitude_std_dev();
60 double altitude_std = bestgnsspos->height_std_dev();
61 pose.local_std =
62 std::sqrt(latitude_std * latitude_std + longitude_std * longitude_std +
63 altitude_std * altitude_std);
66 pose.tz = pose.altitude;
67 sp_pj_transformer_->LatlongToUtm(1, 1, &pose.tx, &pose.ty, &pose.tz);
68
69 std::lock_guard<std::mutex> mutex_locker(mutex_);
70 static FILE *pose_file = fopen("poses.txt", "w");
71 static int count = 0;
72 fprintf(stderr, "%d:%lf %lf %lf %lf 0.0 0.0 0.0 0.0\n", ++count,
73 pose.time_stamp, pose.tx, pose.ty, pose.tz);
74 fprintf(pose_file, "%lf %lf %lf %lf 0.0 0.0 0.0 0.0\n", pose.time_stamp,
75 pose.tx, pose.ty, pose.tz);
76 fflush(pose_file);
77 sp_pose_collection_->Collect(pose);
78}
T GpsToUnixSeconds(T gps_seconds)
constexpr double kDEGRESS_TO_RADIANS