Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] committed Mar 7, 2024
1 parent 57ab852 commit f50a5ca
Show file tree
Hide file tree
Showing 4 changed files with 111 additions and 105 deletions.
2 changes: 1 addition & 1 deletion launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,6 @@
</group>

<group if="$(var use_autoware_pose_covariance_modifier)">
<include file="$(find-pkg-share autoware_pose_covariance_modifier_node)/launch/autoware_pose_covariance_modifier_node.launch.xml" />
<include file="$(find-pkg-share autoware_pose_covariance_modifier_node)/launch/autoware_pose_covariance_modifier_node.launch.xml"/>
</group>
</launch>
7 changes: 5 additions & 2 deletions ...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,10 @@
<?xml version="1.0"?>
<launch>

<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"/>
<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"
/>
<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"/>

<group>
Expand Down
194 changes: 99 additions & 95 deletions ...ion/autoware_pose_covariance_modifier_node/src/autoware_pose_covariance_modifier_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,8 @@

#include <rclcpp/rclcpp.hpp>

AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode() : Node("AutowarePoseCovarianceModifierNode")
AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode()
: Node("AutowarePoseCovarianceModifierNode")
{
trusted_pose_with_cov_sub_ =
this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
Expand All @@ -25,110 +26,113 @@ AutowarePoseCovarianceModifierNode::AutowarePoseCovarianceModifierNode() : Node(
&AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback, this,
std::placeholders::_1));

ndt_pose_with_cov_sub_ =
this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"input_ndt_pose_with_cov_topic", 10000,
std::bind(
&AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback, this,
std::placeholders::_1));
new_pose_estimator_pub_ =
this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"output_pose_with_covariance_topic", 10);

pose_source_pub_ =
this->create_publisher<std_msgs::msg::String>(
"autoware_pose_covariance_modifier/selected_pose_type",10);
ndt_pose_with_cov_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"input_ndt_pose_with_cov_topic", 10000,
std::bind(
&AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback, this,
std::placeholders::_1));
new_pose_estimator_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"output_pose_with_covariance_topic", 10);

pose_source_pub_ = this->create_publisher<std_msgs::msg::String>(
"autoware_pose_covariance_modifier/selected_pose_type", 10);
}
bool AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()
{
auto timeDiff = this->now() - trustedPoseCallbackTime;
if (timeDiff.seconds() > 1.0) {
return true;
}
return false;
auto timeDiff = this->now() - trustedPoseCallbackTime;
if (timeDiff.seconds() > 1.0) {
return true;
}
return false;
}
void AutowarePoseCovarianceModifierNode::publishPoseSource(){

std_msgs::msg::String selected_pose_type;
switch (pose_source_) {
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS):
selected_pose_type.data = "GNSS";
break;
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT):
selected_pose_type.data = "GNSS_NDT";
break;
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT):
selected_pose_type.data = "NDT";
break;
default:
selected_pose_type.data = "Unknown";
break;
}
pose_source_pub_->publish(selected_pose_type);
void AutowarePoseCovarianceModifierNode::publishPoseSource()
{
std_msgs::msg::String selected_pose_type;
switch (pose_source_) {
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS):
selected_pose_type.data = "GNSS";
break;
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT):
selected_pose_type.data = "GNSS_NDT";
break;
case static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT):
selected_pose_type.data = "NDT";
break;
default:
selected_pose_type.data = "Unknown";
break;
}
pose_source_pub_->publish(selected_pose_type);
}
std::array<double, 36> AutowarePoseCovarianceModifierNode::ndt_covariance_modifier(
std::array<double, 36> & in_ndt_covariance)
std::array<double, 36> & in_ndt_covariance)
{
std::array<double, 36> ndt_covariance = in_ndt_covariance;

double position_error_threshold_1_ = this->declare_parameter<double>("PositionErrorThreshold_1",0.10);
double position_error_threshold_2_ = this->declare_parameter<double>("PositionErrorThreshold_2",0.25);
double yaw_error_threshold_ = this->declare_parameter<double>("YawErrorThreshold",0.3);

if (AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()) {
RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
return ndt_covariance;
}

if (trustedPose.pose_average_rmse_xy <= position_error_threshold_1_ &&
trustedPose.yaw_rmse_in_degrees < yaw_error_threshold_) {
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
} else if (trustedPose.pose_average_rmse_xy <= position_error_threshold_2_) {
/*
* ndt_min_rmse_meters = in_ndt_rmse
* ndt_max_rmse_meters = in_ndt_rmse * 2
* ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) / normalization_coeff)
*/
double normalization_coeff = 0.1;
ndt_covariance[0] = std::pow(
((std::sqrt(in_ndt_covariance[0]) * 2) - std::sqrt(trusted_source_pose_with_cov.pose.covariance[0])) *
(std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
2);
ndt_covariance[7] = std::pow(
((std::sqrt(in_ndt_covariance[7]) * 2) - std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) *
(std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
2);
ndt_covariance[14] = std::pow(
((std::sqrt(in_ndt_covariance[14]) * 2) -
std::sqrt(trusted_source_pose_with_cov.pose.covariance[14])) *
(std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
2);

ndt_covariance[0] = std::max(ndt_covariance[0], in_ndt_covariance[0]);
ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);

pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
} else {
RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used ");
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
}

publishPoseSource();
std::array<double, 36> ndt_covariance = in_ndt_covariance;

double position_error_threshold_1_ =
this->declare_parameter<double>("PositionErrorThreshold_1", 0.10);
double position_error_threshold_2_ =
this->declare_parameter<double>("PositionErrorThreshold_2", 0.25);
double yaw_error_threshold_ = this->declare_parameter<double>("YawErrorThreshold", 0.3);

if (AutowarePoseCovarianceModifierNode::checkTrustedPoseTimeout()) {
RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
return ndt_covariance;
}
void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg){
}

std::array<double, 36> ndt_covariance_in_ = msg->pose.covariance;
if (
trustedPose.pose_average_rmse_xy <= position_error_threshold_1_ &&
trustedPose.yaw_rmse_in_degrees < yaw_error_threshold_) {
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS);
} else if (trustedPose.pose_average_rmse_xy <= position_error_threshold_2_) {
/*
* ndt_min_rmse_meters = in_ndt_rmse
* ndt_max_rmse_meters = in_ndt_rmse * 2
* ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) /
* normalization_coeff)
*/
double normalization_coeff = 0.1;
ndt_covariance[0] = std::pow(
((std::sqrt(in_ndt_covariance[0]) * 2) -
std::sqrt(trusted_source_pose_with_cov.pose.covariance[0])) *
(std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
2);
ndt_covariance[7] = std::pow(
((std::sqrt(in_ndt_covariance[7]) * 2) -
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) *
(std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
2);
ndt_covariance[14] = std::pow(
((std::sqrt(in_ndt_covariance[14]) * 2) -
std::sqrt(trusted_source_pose_with_cov.pose.covariance[14])) *
(std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
2);

ndt_covariance[0] = std::max(ndt_covariance[0], in_ndt_covariance[0]);
ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);

pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::GNSS_NDT);
} else {
RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used ");
pose_source_ = static_cast<int>(AutowarePoseCovarianceModifierNode::PoseSource::NDT);
}

publishPoseSource();

return ndt_covariance;
}
void AutowarePoseCovarianceModifierNode::ndt_pose_with_cov_callback(
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
{
std::array<double, 36> ndt_covariance_in_ = msg->pose.covariance;
std::array<double, 36> out_ndt_covariance_ = ndt_covariance_modifier(ndt_covariance_in_);

geometry_msgs::msg::PoseWithCovarianceStamped ndt_pose_with_covariance = *msg;
ndt_pose_with_covariance.pose.covariance = out_ndt_covariance_;
if(pose_source_ != 0){
new_pose_estimator_pub_->publish(ndt_pose_with_covariance);
if (pose_source_ != 0) {
new_pose_estimator_pub_->publish(ndt_pose_with_covariance);
}
}
void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
Expand All @@ -138,14 +142,14 @@ void AutowarePoseCovarianceModifierNode::trusted_pose_with_cov_callback(
trusted_source_pose_with_cov = *msg;

trustedPose.pose_average_rmse_xy = (std::sqrt(trusted_source_pose_with_cov.pose.covariance[0]) +
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) /
2;
std::sqrt(trusted_source_pose_with_cov.pose.covariance[7])) /
2;

trustedPose.yaw_rmse_in_degrees =
std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;
std::sqrt(trusted_source_pose_with_cov.pose.covariance[35]) * 180 / M_PI;

if(pose_source_ != 2) {
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
if (pose_source_ != 2) {
new_pose_estimator_pub_->publish(trusted_source_pose_with_cov);
}
}
int main(int argc, char * argv[])
Expand Down
13 changes: 6 additions & 7 deletions ...ware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,45 +18,44 @@

#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <std_msgs/msg/string.hpp>

#include <string>

class AutowarePoseCovarianceModifierNode : public rclcpp::Node
{
public:
AutowarePoseCovarianceModifierNode();
AutowarePoseCovarianceModifierNode();

rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
trusted_pose_with_cov_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
ndt_pose_with_cov_sub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
new_pose_estimator_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr
pose_source_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_source_pub_;

void trusted_pose_with_cov_callback(
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
void ndt_pose_with_cov_callback(
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
std::array<double, 36> ndt_covariance_modifier(std::array<double, 36> & in_ndt_covariance);
geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_with_cov;
geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_with_cov;

bool checkTrustedPoseTimeout();

private:

struct trusted_pose_
{
double pose_average_rmse_xy = 0.0;
double yaw_rmse_in_degrees = 0.0;
} trustedPose;

enum class PoseSource{
enum class PoseSource {
GNSS = 0,
GNSS_NDT = 1,
NDT = 2,
};
void publishPoseSource();
void publishPoseSource();
int pose_source_;
rclcpp::Time trustedPoseCallbackTime;
};
Expand Down

0 comments on commit f50a5ca

Please sign in to comment.