@@ -125,16 +125,17 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
125
125
trustedPoseCallbackTime = this ->now ();
126
126
trusted_source_pose_with_cov = *msg;
127
127
128
- double trusted_pose_average_rmse_xy;
129
- double trusted_pose_yaw_rmse_in_degrees;
130
- trusted_pose_average_rmse_xy = (std::sqrt (trusted_source_pose_with_cov.pose .covariance [0 ]) +
128
+ // double trusted_pose_average_rmse_xy;
129
+ // double trusted_pose_yaw_rmse_in_degrees;
130
+ double trusted_pose_average_rmse_xy = (std::sqrt (trusted_source_pose_with_cov.pose .covariance [0 ]) +
131
131
std::sqrt (trusted_source_pose_with_cov.pose .covariance [7 ])) /
132
132
2 ;
133
133
134
- trusted_pose_yaw_rmse_in_degrees =
134
+ double trusted_pose_rmse_z = std::sqrt (trusted_source_pose_with_cov.pose .covariance [0 ]);
135
+ double trusted_pose_yaw_rmse_in_degrees =
135
136
std::sqrt (trusted_source_pose_with_cov.pose .covariance [35 ]) * 180 / M_PI;
136
137
137
- selectPositionSource (trusted_pose_average_rmse_xy, trusted_pose_yaw_rmse_in_degrees);
138
+ selectPositionSource (trusted_pose_average_rmse_xy, trusted_pose_yaw_rmse_in_degrees,trusted_pose_rmse_z );
138
139
139
140
if (pose_source_ != 2 ) {
140
141
new_pose_estimator_pub_->publish (trusted_source_pose_with_cov);
@@ -147,15 +148,15 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
147
148
}
148
149
149
150
void AutowarePoseCovarianceModifierNode::selectPositionSource (
150
- double trusted_pose_average_rmse_xy, double trusted_pose_yaw_rmse_in_degrees)
151
+ double trusted_pose_average_rmse_xy, double trusted_pose_yaw_rmse_in_degrees, double trusted_pose_rmse_z )
151
152
{
152
153
std_msgs::msg::String selected_pose_type;
153
154
if (
154
155
trusted_pose_average_rmse_xy <= gnss_error_reliable_max_ &&
155
- trusted_pose_yaw_rmse_in_degrees < yaw_error_deg_threshold_) {
156
+ trusted_pose_yaw_rmse_in_degrees < yaw_error_deg_threshold_ && trusted_pose_rmse_z < gnss_error_reliable_max_ ) {
156
157
pose_source_ = static_cast <int >(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
157
158
selected_pose_type.data = " GNSS" ;
158
- } else if (trusted_pose_average_rmse_xy <= gnss_error_unreliable_min_) {
159
+ } else if (trusted_pose_average_rmse_xy <= gnss_error_unreliable_min_ && trusted_pose_rmse_z < gnss_error_unreliable_min_ ) {
159
160
pose_source_ = static_cast <int >(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
160
161
selected_pose_type.data = " GNSS + NDT" ;
161
162
} else {
0 commit comments