Skip to content

Commit e531604

Browse files
check position rmse for z
Signed-off-by: meliketanrikulu <melike@leodrive.ai>
1 parent 6399278 commit e531604

File tree

3 files changed

+13
-12
lines changed

3 files changed

+13
-12
lines changed
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,14 @@
11
/**:
22
ros__parameters:
33
error_thresholds:
4-
# Threshold value for the range in which GNSS error is most reliable
4+
# Threshold value for the range in which GNSS position error is most reliable
55
gnss_error_reliable_max: 0.1 #meters
66

7-
# Threshold value at which GNSS error is not considered reliable
7+
# Threshold value at which GNSS position error is not considered reliable
88
gnss_error_unreliable_min: 0.25 #meters
99

1010
# Threshold value for yaw error
1111
yaw_error_deg_threshold: 0.3 #degrees
1212

1313
debug:
14-
enable_debug_topics: false
14+
enable_debug_topics: true

localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp

+9-8
Original file line numberDiff line numberDiff line change
@@ -125,16 +125,17 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
125125
trustedPoseCallbackTime = this->now();
126126
trusted_source_pose_with_cov = *msg;
127127

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]) +
131131
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) /
132132
2;
133133

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 =
135136
std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;
136137

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);
138139

139140
if (pose_source_ != 2) {
140141
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
@@ -147,15 +148,15 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
147148
}
148149

149150
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)
151152
{
152153
std_msgs::msg::String selected_pose_type;
153154
if (
154155
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_) {
156157
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
157158
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_) {
159160
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
160161
selected_pose_type.data = "GNSS + NDT";
161162
} else {

localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ class AutowarePoseCovarianceModifierNode : public rclcpp::Node
4848

4949
private:
5050
void selectPositionSource(
51-
double trusted_pose_average_rmse_xy, double trusted_pose_yaw_rmse_in_degrees);
51+
double trusted_pose_average_rmse_xy, double trusted_pose_yaw_rmse_in_degrees, double trusted_pose_average_rmse_z);
5252

5353
enum class PoseSource {
5454
GNSS = 0,

0 commit comments

Comments
 (0)