Skip to content

Commit f50a5ca

Browse files
pre-commit-ci[bot]pre-commit-ci[bot]
committed
style(pre-commit): autofix
1 parent 57ab852 commit f50a5ca

File tree

4 files changed

+111
-105
lines changed

4 files changed

+111
-105
lines changed

‎launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,6 @@
115115
</group>
116116

117117
<group if="$(var use_autoware_pose_covariance_modifier)">
118-
<include file="$(find-pkg-share autoware_pose_covariance_modifier_node)/launch/autoware_pose_covariance_modifier_node.launch.xml" />
118+
<include file="$(find-pkg-share autoware_pose_covariance_modifier_node)/launch/autoware_pose_covariance_modifier_node.launch.xml"/>
119119
</group>
120120
</launch>

‎launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

+5-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,10 @@
11
<?xml version="1.0"?>
22
<launch>
3-
4-
<let name="ekf_input_pose_with_cov_name" if="$(eval &quot;'$(var use_autoware_pose_covariance_modifier)'=='true'&quot;)" value="/localization/autoware_pose_covariance_modifier/pose_with_covariance"/>
3+
<let
4+
name="ekf_input_pose_with_cov_name"
5+
if="$(eval &quot;'$(var use_autoware_pose_covariance_modifier)'=='true'&quot;)"
6+
value="/localization/autoware_pose_covariance_modifier/pose_with_covariance"
7+
/>
58
<let name="ekf_input_pose_with_cov_name" if="$(eval &quot;'$(var use_autoware_pose_covariance_modifier)'=='false'&quot;)" value="/localization/pose_estimator/pose_with_covariance"/>
69

710
<group>

‎localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp

+99-95
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,8 @@
1616

1717
#include <rclcpp/rclcpp.hpp>
1818

19-
AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode() : Node("AutowarePoseCovarianceModifierNode")
19+
AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode()
20+
: Node("AutowarePoseCovarianceModifierNode")
2021
{
2122
trusted_pose_with_cov_sub_ =
2223
this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
@@ -25,110 +26,113 @@ AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode() : Node(
2526
&AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback, this,
2627
std::placeholders::_1));
2728

28-
ndt_pose_with_cov_sub_ =
29-
this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
30-
"input_ndt_pose_with_cov_topic", 10000,
31-
std::bind(
32-
&AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback, this,
33-
std::placeholders::_1));
34-
new_pose_estimator_pub_ =
35-
this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
36-
"output_pose_with_covariance_topic", 10);
37-
38-
pose_source_pub_ =
39-
this->create_publisher<std_msgs::msg::String>(
40-
"autoware_pose_covariance_modifier/selected_pose_type",10);
29+
ndt_pose_with_cov_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
30+
"input_ndt_pose_with_cov_topic", 10000,
31+
std::bind(
32+
&AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback, this,
33+
std::placeholders::_1));
34+
new_pose_estimator_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
35+
"output_pose_with_covariance_topic", 10);
4136

37+
pose_source_pub_ = this->create_publisher<std_msgs::msg::String>(
38+
"autoware_pose_covariance_modifier/selected_pose_type", 10);
4239
}
4340
bool AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()
4441
{
45-
auto timeDiff = this->now() - trustedPoseCallbackTime;
46-
if (timeDiff.seconds() > 1.0) {
47-
return true;
48-
}
49-
return false;
42+
auto timeDiff = this->now() - trustedPoseCallbackTime;
43+
if (timeDiff.seconds() > 1.0) {
44+
return true;
45+
}
46+
return false;
5047
}
51-
void AutowarePoseCovarianceModifierNode::publishPoseSource(){
52-
53-
std_msgs::msg::String selected_pose_type;
54-
switch (pose_source_) {
55-
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS):
56-
selected_pose_type.data = "GNSS";
57-
break;
58-
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT):
59-
selected_pose_type.data = "GNSS_NDT";
60-
break;
61-
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT):
62-
selected_pose_type.data = "NDT";
63-
break;
64-
default:
65-
selected_pose_type.data = "Unknown";
66-
break;
67-
}
68-
pose_source_pub_->publish(selected_pose_type);
48+
void AutowarePoseCovarianceModifierNode::publishPoseSource()
49+
{
50+
std_msgs::msg::String selected_pose_type;
51+
switch (pose_source_) {
52+
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS):
53+
selected_pose_type.data = "GNSS";
54+
break;
55+
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT):
56+
selected_pose_type.data = "GNSS_NDT";
57+
break;
58+
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT):
59+
selected_pose_type.data = "NDT";
60+
break;
61+
default:
62+
selected_pose_type.data = "Unknown";
63+
break;
64+
}
65+
pose_source_pub_->publish(selected_pose_type);
6966
}
7067
std::array<double, 36> AutowarePoseCovarianceModifierNode::ndt_covariance_modifier(
71-
std::array<double, 36> & in_ndt_covariance)
68+
std::array<double, 36> & in_ndt_covariance)
7269
{
73-
std::array<double, 36> ndt_covariance = in_ndt_covariance;
74-
75-
double position_error_threshold_1_ = this->declare_parameter<double>("PositionErrorThreshold_1",0.10);
76-
double position_error_threshold_2_ = this->declare_parameter<double>("PositionErrorThreshold_2",0.25);
77-
double yaw_error_threshold_ = this->declare_parameter<double>("YawErrorThreshold",0.3);
78-
79-
if (AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()) {
80-
RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
81-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
82-
return ndt_covariance;
83-
}
84-
85-
if (trustedPose.pose_average_rmse_xy <= position_error_threshold_1_ &&
86-
trustedPose.yaw_rmse_in_degrees < yaw_error_threshold_) {
87-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
88-
} else if (trustedPose.pose_average_rmse_xy <= position_error_threshold_2_) {
89-
/*
90-
* ndt_min_rmse_meters = in_ndt_rmse
91-
* ndt_max_rmse_meters = in_ndt_rmse * 2
92-
* ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) / normalization_coeff)
93-
*/
94-
double normalization_coeff = 0.1;
95-
ndt_covariance[0] = std::pow(
96-
((std::sqrt(in_ndt_covariance[0]) * 2) - std::sqrt(trusted_source_pose_with_cov.pose.covariance[0])) *
97-
(std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
98-
2);
99-
ndt_covariance[7] = std::pow(
100-
((std::sqrt(in_ndt_covariance[7]) * 2) - std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) *
101-
(std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
102-
2);
103-
ndt_covariance[14] = std::pow(
104-
((std::sqrt(in_ndt_covariance[14]) * 2) -
105-
std::sqrt(trusted_source_pose_with_cov.pose.covariance[14])) *
106-
(std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
107-
2);
108-
109-
ndt_covariance[0] = std::max(ndt_covariance[0], in_ndt_covariance[0]);
110-
ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
111-
ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);
112-
113-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
114-
} else {
115-
RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used ");
116-
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
117-
}
118-
119-
publishPoseSource();
70+
std::array<double, 36> ndt_covariance = in_ndt_covariance;
12071

72+
double position_error_threshold_1_ =
73+
this->declare_parameter<double>("PositionErrorThreshold_1", 0.10);
74+
double position_error_threshold_2_ =
75+
this->declare_parameter<double>("PositionErrorThreshold_2", 0.25);
76+
double yaw_error_threshold_ = this->declare_parameter<double>("YawErrorThreshold", 0.3);
77+
78+
if (AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()) {
79+
RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
80+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
12181
return ndt_covariance;
122-
}
123-
void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg){
82+
}
12483

125-
std::array<double, 36> ndt_covariance_in_ = msg->pose.covariance;
84+
if (
85+
trustedPose.pose_average_rmse_xy <= position_error_threshold_1_ &&
86+
trustedPose.yaw_rmse_in_degrees < yaw_error_threshold_) {
87+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
88+
} else if (trustedPose.pose_average_rmse_xy <= position_error_threshold_2_) {
89+
/*
90+
* ndt_min_rmse_meters = in_ndt_rmse
91+
* ndt_max_rmse_meters = in_ndt_rmse * 2
92+
* ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) /
93+
* normalization_coeff)
94+
*/
95+
double normalization_coeff = 0.1;
96+
ndt_covariance[0] = std::pow(
97+
((std::sqrt(in_ndt_covariance[0]) * 2) -
98+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[0])) *
99+
(std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
100+
2);
101+
ndt_covariance[7] = std::pow(
102+
((std::sqrt(in_ndt_covariance[7]) * 2) -
103+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) *
104+
(std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
105+
2);
106+
ndt_covariance[14] = std::pow(
107+
((std::sqrt(in_ndt_covariance[14]) * 2) -
108+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[14])) *
109+
(std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
110+
2);
111+
112+
ndt_covariance[0] = std::max(ndt_covariance[0], in_ndt_covariance[0]);
113+
ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
114+
ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);
115+
116+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
117+
} else {
118+
RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used ");
119+
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
120+
}
121+
122+
publishPoseSource();
123+
124+
return ndt_covariance;
125+
}
126+
void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(
127+
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
128+
{
129+
std::array<double, 36> ndt_covariance_in_ = msg->pose.covariance;
126130
std::array<double, 36> out_ndt_covariance_ = ndt_covariance_modifier(ndt_covariance_in_);
127131

128132
geometry_msgs::msg::PoseWithCovarianceStamped ndt_pose_with_covariance = *msg;
129133
ndt_pose_with_covariance.pose.covariance = out_ndt_covariance_;
130-
if(pose_source_ != 0){
131-
new_pose_estimator_pub_->publish(ndt_pose_with_covariance);
134+
if (pose_source_ != 0) {
135+
new_pose_estimator_pub_->publish(ndt_pose_with_covariance);
132136
}
133137
}
134138
void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
@@ -138,14 +142,14 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
138142
trusted_source_pose_with_cov = *msg;
139143

140144
trustedPose.pose_average_rmse_xy = (std::sqrt(trusted_source_pose_with_cov.pose.covariance[0]) +
141-
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) /
142-
2;
145+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) /
146+
2;
143147

144148
trustedPose.yaw_rmse_in_degrees =
145-
std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;
149+
std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;
146150

147-
if(pose_source_ != 2) {
148-
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
151+
if (pose_source_ != 2) {
152+
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
149153
}
150154
}
151155
int main(int argc, char * argv[])

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

+6-7
Original file line numberDiff line numberDiff line change
@@ -18,45 +18,44 @@
1818

1919
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
2020
#include <std_msgs/msg/string.hpp>
21+
2122
#include <string>
2223

2324
class AutowarePoseCovarianceModifierNode : public rclcpp::Node
2425
{
2526
public:
26-
AutowarePoseCovarianceModifierNode();
27+
AutowarePoseCovarianceModifierNode();
2728

2829
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
2930
trusted_pose_with_cov_sub_;
3031
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
3132
ndt_pose_with_cov_sub_;
3233
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
3334
new_pose_estimator_pub_;
34-
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr
35-
pose_source_pub_;
35+
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_source_pub_;
3636

3737
void trusted_pose_with_cov_callback(
3838
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
3939
void ndt_pose_with_cov_callback(
4040
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
4141
std::array<double, 36> ndt_covariance_modifier(std::array<double, 36> & in_ndt_covariance);
42-
geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_with_cov;
42+
geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_with_cov;
4343

4444
bool checkTrustedPoseTimeout();
4545

4646
private:
47-
4847
struct trusted_pose_
4948
{
5049
double pose_average_rmse_xy = 0.0;
5150
double yaw_rmse_in_degrees = 0.0;
5251
} trustedPose;
5352

54-
enum class PoseSource{
53+
enum class PoseSource {
5554
GNSS = 0,
5655
GNSS_NDT = 1,
5756
NDT = 2,
5857
};
59-
void publishPoseSource();
58+
void publishPoseSource();
6059
int pose_source_;
6160
rclcpp::Time trustedPoseCallbackTime;
6261
};

0 commit comments

Comments
 (0)