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