Skip to content

Commit 57ad9b1

Browse files
style(pre-commit): autofix
1 parent e531604 commit 57ad9b1

File tree

2 files changed

+20
-13
lines changed

2 files changed

+20
-13
lines changed

localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp

+18-12
Original file line numberDiff line numberDiff line change
@@ -125,17 +125,19 @@ 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-
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 =
136137
std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;
137138

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

140142
if (pose_source_ != 2) {
141143
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
@@ -148,15 +150,19 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
148150
}
149151

150152
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)
152155
{
153156
std_msgs::msg::String selected_pose_type;
154157
if (
155158
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_) {
157161
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
158162
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_) {
160166
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
161167
selected_pose_type.data = "GNSS + NDT";
162168
} else {

localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,8 @@ 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, double trusted_pose_average_rmse_z);
51+
double trusted_pose_average_rmse_xy, double trusted_pose_yaw_rmse_in_degrees,
52+
double trusted_pose_average_rmse_z);
5253

5354
enum class PoseSource {
5455
GNSS = 0,

0 commit comments

Comments
 (0)