Skip to content

Commit 3e35850

Browse files
pre-commit-ci[bot]meliketanrikulu
pre-commit-ci[bot]
authored and
meliketanrikulu
committed
style(pre-commit): autofix
1 parent cfc4616 commit 3e35850

File tree

4 files changed

+86
-77
lines changed

4 files changed

+86
-77
lines changed

‎localization/autoware_pose_covariance_modifier_node/README.md

+7-7
Original file line numberDiff line numberDiff line change
@@ -35,17 +35,17 @@ To activate, you need to change the `use_autoware_pose_covariance_modifier` para
3535

3636
## Published Topics
3737

38-
| Name | Type | Description |
39-
|-------------------------------------|-------------------------------------------------|----------------------------------------------------------------------------------------------------------|
40-
| `output_pose_with_covariance_topic` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Output pose topic. It will be sent as input to ekf_localizer package. |
41-
| `selected_pose_type` | `std_msgs::msg::String` | Declares which pose sources are used in the output of this package |
42-
| `output/ndt_position_rmse` | `std_msgs::msg::Float32` | Output pose ndt avarage rmse in position xy. It is published only when the enable_debug_topics is true. |
43-
| `output/gnss_position_rmse` | `std_msgs::msg::Float32` | Output pose gnss avarage rmse in position xy.It is published only when the enable_debug_topics is true. |
38+
| Name | Type | Description |
39+
| ----------------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------------------------- |
40+
| `output_pose_with_covariance_topic` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Output pose topic. It will be sent as input to ekf_localizer package. |
41+
| `selected_pose_type` | `std_msgs::msg::String` | Declares which pose sources are used in the output of this package |
42+
| `output/ndt_position_rmse` | `std_msgs::msg::Float32` | Output pose ndt avarage rmse in position xy. It is published only when the enable_debug_topics is true. |
43+
| `output/gnss_position_rmse` | `std_msgs::msg::Float32` | Output pose gnss avarage rmse in position xy.It is published only when the enable_debug_topics is true. |
4444

4545
## Parameters
4646

4747
| Name | Type | Description |
48-
|----------------------------------------------|----------|--------------------------------------------------------------------------------|
48+
| -------------------------------------------- | -------- | ------------------------------------------------------------------------------ |
4949
| `error_thresholds.gnss_error_reliable_max` | `double` | Threshold value for the range in which GNSS error is most reliable. |
5050
| `error_thresholds.gnss_error_unreliable_min` | `double` | Threshold value at which GNSS error is not considered reliable. |
5151
| `error_thresholds.yaw_error_deg_threshold` | `double` | Threshold value to understand whether the yaw error is within reliable limits. |

‎localization/autoware_pose_covariance_modifier_node/config/autoware_pose_covariance_modifier.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,4 +11,4 @@
1111
yaw_error_deg_threshold: 0.3 #degrees
1212

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

‎localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp

+74-66
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,12 @@
1919
AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode()
2020
: Node("AutowarePoseCovarianceModifierNode")
2121
{
22-
gnss_error_reliable_max_ = this->declare_parameter<double>("error_thresholds.gnss_error_reliable_max", 0.10);
23-
gnss_error_unreliable_min_ = this->declare_parameter<double>("error_thresholds.gnss_error_unreliable_min", 0.25);
24-
yaw_error_deg_threshold_ = this->declare_parameter<double>("error_thresholds.yaw_error_deg_threshold", 0.3);
22+
gnss_error_reliable_max_ =
23+
this->declare_parameter<double>("error_thresholds.gnss_error_reliable_max", 0.10);
24+
gnss_error_unreliable_min_ =
25+
this->declare_parameter<double>("error_thresholds.gnss_error_unreliable_min", 0.25);
26+
yaw_error_deg_threshold_ =
27+
this->declare_parameter<double>("error_thresholds.yaw_error_deg_threshold", 0.3);
2528
debug_ = this->declare_parameter<bool>("debug.enable_debug_topics", false);
2629

2730
trusted_pose_with_cov_sub_ =
@@ -41,23 +44,22 @@ AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode()
4144

4245
pose_source_pub_ = this->create_publisher<std_msgs::msg::String>(
4346
"autoware_pose_covariance_modifier/selected_pose_type", 10);
44-
if (debug_){
45-
out_ndt_position_rmse_pub_ = this->create_publisher<std_msgs::msg::Float32>(
46-
"/autoware_pose_covariance_modifier/output/ndt_position_rmse", 10);
47-
out_gnss_position_rmse_pub_ = this->create_publisher<std_msgs::msg::Float32>(
48-
"/autoware_pose_covariance_modifier/output/gnss_position_rmse", 10);
47+
if (debug_) {
48+
out_ndt_position_rmse_pub_ = this->create_publisher<std_msgs::msg::Float32>(
49+
"/autoware_pose_covariance_modifier/output/ndt_position_rmse", 10);
50+
out_gnss_position_rmse_pub_ = this->create_publisher<std_msgs::msg::Float32>(
51+
"/autoware_pose_covariance_modifier/output/gnss_position_rmse", 10);
4952
}
50-
5153
}
5254
void AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()
5355
{
5456
auto timeDiff = this->now() - trustedPoseCallbackTime;
5557
if (timeDiff.seconds() > 1.0) {
56-
RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
57-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
58-
std_msgs::msg::String selected_pose_type;
59-
selected_pose_type.data = "NDT";
60-
pose_source_pub_->publish(selected_pose_type);
58+
RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
59+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
60+
std_msgs::msg::String selected_pose_type;
61+
selected_pose_type.data = "NDT";
62+
pose_source_pub_->publish(selected_pose_type);
6163
}
6264
}
6365

@@ -66,32 +68,32 @@ std::array<double, 36> AutowarePoseCovarianceModifierNode::ndt_covariance_modifi
6668
{
6769
std::array<double, 36> ndt_covariance = in_ndt_covariance;
6870

69-
/*
70-
* ndt_min_rmse_meters = in_ndt_rmse
71-
* ndt_max_rmse_meters = in_ndt_rmse * 2
72-
* ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) /
73-
* normalization_coeff)
74-
*/
75-
double normalization_coeff = 0.1;
76-
ndt_covariance[0] = std::pow(
77-
((std::sqrt(in_ndt_covariance[0]) * 2) -
78-
std::sqrt(trusted_source_pose_with_cov.pose.covariance[0])) *
79-
(std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
80-
2);
81-
ndt_covariance[7] = std::pow(
82-
((std::sqrt(in_ndt_covariance[7]) * 2) -
83-
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) *
84-
(std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
85-
2);
86-
ndt_covariance[14] = std::pow(
87-
((std::sqrt(in_ndt_covariance[14]) * 2) -
88-
std::sqrt(trusted_source_pose_with_cov.pose.covariance[14])) *
89-
(std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
90-
2);
91-
92-
ndt_covariance[0] = std::max(ndt_covariance[0], in_ndt_covariance[0]);
93-
ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
94-
ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);
71+
/*
72+
* ndt_min_rmse_meters = in_ndt_rmse
73+
* ndt_max_rmse_meters = in_ndt_rmse * 2
74+
* ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) /
75+
* normalization_coeff)
76+
*/
77+
double normalization_coeff = 0.1;
78+
ndt_covariance[0] = std::pow(
79+
((std::sqrt(in_ndt_covariance[0]) * 2) -
80+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[0])) *
81+
(std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
82+
2);
83+
ndt_covariance[7] = std::pow(
84+
((std::sqrt(in_ndt_covariance[7]) * 2) -
85+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) *
86+
(std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
87+
2);
88+
ndt_covariance[14] = std::pow(
89+
((std::sqrt(in_ndt_covariance[14]) * 2) -
90+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[14])) *
91+
(std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
92+
2);
93+
94+
ndt_covariance[0] = std::max(ndt_covariance[0], in_ndt_covariance[0]);
95+
ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
96+
ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);
9597

9698
return ndt_covariance;
9799
}
@@ -101,16 +103,19 @@ void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(
101103
AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout();
102104

103105
std::array<double, 36> ndt_covariance_in_ = msg->pose.covariance;
104-
std::array<double, 36> out_ndt_covariance_ = pose_source_ == 1 ? ndt_covariance_modifier(ndt_covariance_in_) : ndt_covariance_in_ ;
106+
std::array<double, 36> out_ndt_covariance_ =
107+
pose_source_ == 1 ? ndt_covariance_modifier(ndt_covariance_in_) : ndt_covariance_in_;
105108

106109
geometry_msgs::msg::PoseWithCovarianceStamped ndt_pose_with_covariance = *msg;
107110
ndt_pose_with_covariance.pose.covariance = out_ndt_covariance_;
108111
if (pose_source_ != 0) {
109112
new_pose_estimator_pub_->publish(ndt_pose_with_covariance);
110-
if(debug_){
111-
std_msgs::msg::Float32 out_ndt_rmse;
112-
out_ndt_rmse.data = (std::sqrt(ndt_pose_with_covariance.pose.covariance[0]) + std::sqrt(ndt_pose_with_covariance.pose.covariance[7])) / 2;
113-
out_ndt_position_rmse_pub_->publish(out_ndt_rmse);
113+
if (debug_) {
114+
std_msgs::msg::Float32 out_ndt_rmse;
115+
out_ndt_rmse.data = (std::sqrt(ndt_pose_with_covariance.pose.covariance[0]) +
116+
std::sqrt(ndt_pose_with_covariance.pose.covariance[7])) /
117+
2;
118+
out_ndt_position_rmse_pub_->publish(out_ndt_rmse);
114119
}
115120
}
116121
}
@@ -122,9 +127,9 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
122127

123128
double trusted_pose_average_rmse_xy;
124129
double trusted_pose_yaw_rmse_in_degrees;
125-
trusted_pose_average_rmse_xy= (std::sqrt(trusted_source_pose_with_cov.pose.covariance[0]) +
126-
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) /
127-
2;
130+
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;
128133

129134
trusted_pose_yaw_rmse_in_degrees =
130135
std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;
@@ -133,28 +138,31 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
133138

134139
if (pose_source_ != 2) {
135140
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
136-
if(debug_){
137-
std_msgs::msg::Float32 out_gnss_rmse;
138-
out_gnss_rmse.data = trusted_pose_average_rmse_xy;
139-
out_gnss_position_rmse_pub_->publish(out_gnss_rmse);
141+
if (debug_) {
142+
std_msgs::msg::Float32 out_gnss_rmse;
143+
out_gnss_rmse.data = trusted_pose_average_rmse_xy;
144+
out_gnss_position_rmse_pub_->publish(out_gnss_rmse);
140145
}
141146
}
142147
}
143148

144-
void AutowarePoseCovarianceModifierNode::selectPositionSource(double trusted_pose_average_rmse_xy ,double trusted_pose_yaw_rmse_in_degrees){
145-
std_msgs::msg::String selected_pose_type;
146-
if (trusted_pose_average_rmse_xy <= gnss_error_reliable_max_ &&
147-
trusted_pose_yaw_rmse_in_degrees < yaw_error_deg_threshold_) {
148-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
149-
selected_pose_type.data = "GNSS";
150-
} else if (trusted_pose_average_rmse_xy <= gnss_error_unreliable_min_) {
151-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
152-
selected_pose_type.data = "GNSS + NDT";
153-
} else {
154-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
155-
selected_pose_type.data = "NDT";
156-
}
157-
pose_source_pub_->publish(selected_pose_type);
149+
void AutowarePoseCovarianceModifierNode::selectPositionSource(
150+
double trusted_pose_average_rmse_xy, double trusted_pose_yaw_rmse_in_degrees)
151+
{
152+
std_msgs::msg::String selected_pose_type;
153+
if (
154+
trusted_pose_average_rmse_xy <= gnss_error_reliable_max_ &&
155+
trusted_pose_yaw_rmse_in_degrees < yaw_error_deg_threshold_) {
156+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
157+
selected_pose_type.data = "GNSS";
158+
} else if (trusted_pose_average_rmse_xy <= gnss_error_unreliable_min_) {
159+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
160+
selected_pose_type.data = "GNSS + NDT";
161+
} else {
162+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
163+
selected_pose_type.data = "NDT";
164+
}
165+
pose_source_pub_->publish(selected_pose_type);
158166
}
159167
int main(int argc, char * argv[])
160168
{

‎localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -17,8 +17,9 @@
1717
#include <rclcpp/rclcpp.hpp>
1818

1919
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
20-
#include <std_msgs/msg/string.hpp>
2120
#include <std_msgs/msg/float32.hpp>
21+
#include <std_msgs/msg/string.hpp>
22+
2223
#include <string>
2324

2425
class AutowarePoseCovarianceModifierNode : public rclcpp::Node
@@ -46,8 +47,8 @@ class AutowarePoseCovarianceModifierNode : public rclcpp::Node
4647
void checkTrustedPoseTimeout();
4748

4849
private:
49-
50-
void selectPositionSource(double trusted_pose_average_rmse_xy ,double trusted_pose_yaw_rmse_in_degrees);
50+
void selectPositionSource(
51+
double trusted_pose_average_rmse_xy, double trusted_pose_yaw_rmse_in_degrees);
5152

5253
enum class PoseSource {
5354
GNSS = 0,

0 commit comments

Comments
 (0)