Skip to content

Commit 4cf7dde

Browse files
meliketanrikulumeliketanrikulu
committed
add debug publishers
Signed-off-by: meliketanrikulu <melike@leodrive.ai>
1 parent a46f001 commit 4cf7dde

File tree

4 files changed

+48
-18
lines changed

4 files changed

+48
-18
lines changed

‎localization/autoware_pose_covariance_modifier_node/README.md

+12-8
Original file line numberDiff line numberDiff line change
@@ -35,17 +35,21 @@ 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. |
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. |
4144

4245
## Parameters
4346

44-
| Name | Type | Description |
45-
| --------------------------- | -------- | ------------------------------------------------------------------------------ |
46-
| `gnss_error_reliable_max` | `double` | Threshold value for the range in which GNSS error is most reliable. |
47-
| `gnss_error_unreliable_min` | `double` | Threshold value at which GNSS error is not considered reliable. |
48-
| `yaw_error_deg_threshold` | `double` | Threshold value to understand whether the yaw error is within reliable limits. |
47+
| Name | Type | Description |
48+
|----------------------------------------------|----------|--------------------------------------------------------------------------------|
49+
| `error_thresholds.gnss_error_reliable_max` | `double` | Threshold value for the range in which GNSS error is most reliable. |
50+
| `error_thresholds.gnss_error_unreliable_min` | `double` | Threshold value at which GNSS error is not considered reliable. |
51+
| `error_thresholds.yaw_error_deg_threshold` | `double` | Threshold value to understand whether the yaw error is within reliable limits. |
52+
| `debug.enable_debug_topics` | `bool` | Enables the debug topics |
4953

5054
### Important Notes
5155

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

+10-6
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,14 @@
11
/**:
22
ros__parameters:
3-
# Threshold value for the range in which GNSS error is most reliable
4-
gnss_error_reliable_max: 0.1 #meters
3+
error_thresholds:
4+
# Threshold value for the range in which GNSS error is most reliable
5+
gnss_error_reliable_max: 0.1 #meters
56

6-
# Threshold value at which GNSS error is not considered reliable
7-
gnss_error_unreliable_min: 0.25 #meters
7+
# Threshold value at which GNSS error is not considered reliable
8+
gnss_error_unreliable_min: 0.25 #meters
89

9-
# Threshold value for yaw error
10-
yaw_error_deg_threshold: 0.3 #degrees
10+
# Threshold value for yaw error
11+
yaw_error_deg_threshold: 0.3 #degrees
12+
13+
debug:
14+
enable_debug_topics: false

‎localization/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp

+22-3
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,10 @@
1919
AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode()
2020
: Node("AutowarePoseCovarianceModifierNode")
2121
{
22-
gnss_error_reliable_max_ = this->declare_parameter<double>("gnss_error_reliable_max", 0.10);
23-
gnss_error_unreliable_min_ = this->declare_parameter<double>("gnss_error_unreliable_min", 0.25);
24-
yaw_error_deg_threshold_ = this->declare_parameter<double>("yaw_error_deg_threshold", 0.3);
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);
25+
debug_ = this->declare_parameter<bool>("debug.enable_debug_topics", false);
2526

2627
trusted_pose_with_cov_sub_ =
2728
this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
@@ -40,6 +41,13 @@ AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode()
4041

4142
pose_source_pub_ = this->create_publisher<std_msgs::msg::String>(
4243
"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);
49+
}
50+
4351
}
4452
bool AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()
4553
{
@@ -131,6 +139,11 @@ void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(
131139
ndt_pose_with_covariance.pose.covariance = out_ndt_covariance_;
132140
if (pose_source_ != 0) {
133141
new_pose_estimator_pub_->publish(ndt_pose_with_covariance);
142+
if(debug_){
143+
std_msgs::msg::Float32 out_ndt_rmse;
144+
out_ndt_rmse.data = (std::sqrt(ndt_pose_with_covariance.pose.covariance[0]) + std::sqrt(ndt_pose_with_covariance.pose.covariance[7])) / 2;
145+
out_ndt_position_rmse_pub_->publish(out_ndt_rmse);
146+
}
134147
}
135148
}
136149
void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
@@ -148,6 +161,12 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
148161

149162
if (pose_source_ != 2) {
150163
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
164+
if(debug_){
165+
std_msgs::msg::Float32 out_gnss_rmse;
166+
out_gnss_rmse.data = trustedPose.pose_average_rmse_xy;
167+
out_gnss_position_rmse_pub_->publish(out_gnss_rmse);
168+
}
169+
151170
}
152171
}
153172
int main(int argc, char * argv[])

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

+4-1
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818

1919
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
2020
#include <std_msgs/msg/string.hpp>
21-
21+
#include <std_msgs/msg/float32.hpp>
2222
#include <string>
2323

2424
class AutowarePoseCovarianceModifierNode : public rclcpp::Node
@@ -33,6 +33,8 @@ class AutowarePoseCovarianceModifierNode : public rclcpp::Node
3333
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
3434
new_pose_estimator_pub_;
3535
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_source_pub_;
36+
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr out_ndt_position_rmse_pub_;
37+
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr out_gnss_position_rmse_pub_;
3638

3739
void trusted_pose_with_cov_callback(
3840
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
@@ -60,6 +62,7 @@ class AutowarePoseCovarianceModifierNode : public rclcpp::Node
6062
rclcpp::Time trustedPoseCallbackTime;
6163

6264
double gnss_error_reliable_max_, gnss_error_unreliable_min_, yaw_error_deg_threshold_;
65+
bool debug_;
6366
};
6467

6568
#endif // AUTOWARE_POSE_COVARIANCE_MODIFIER_NODE_HPP_

0 commit comments

Comments
 (0)