Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(ekf_localizer): rename biased pose topics #1787

Merged
merged 6 commits into from
Sep 6, 2022
Merged

fix(ekf_localizer): rename biased pose topics #1787

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e48e7ff
fix(ekf_localizer): rename biased yaw topic
IshitaTakeshi Sep 5, 2022
a61a792
Update topic names and their descriptions in README
IshitaTakeshi Sep 5, 2022
52756aa
ci(pre-commit): autofix
pre-commit-ci[bot] Sep 5, 2022
fd792c0
Fix comments
IshitaTakeshi Sep 6, 2022
71f7c37
Rename variables
IshitaTakeshi Sep 6, 2022
b10afec
ci(pre-commit): autofix
pre-commit-ci[bot] Sep 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
<include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
<arg name="input_map_points_topic" value="/map/pointcloud_map"/>
<arg name="input/pointcloud" value="/localization/util/downsample/pointcloud"/>
<arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"/>
<arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>

<arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
<arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
Expand Down
4 changes: 2 additions & 2 deletions ...4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@
<arg name="output_odom_name" value="kinematic_state"/>
<arg name="output_pose_name" value="pose"/>
<arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
<arg name="output_pose_without_yawbias_name" value="pose_without_yawbias"/>
<arg name="output_pose_with_covariance_without_yawbias_name" value="pose_with_covariance_without_yawbias"/>
<arg name="output_biased_pose_name" value="biased_pose"/>
<arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
<arg name="output_twist_name" value="twist"/>
<arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
<arg name="proc_stddev_vx_c" value="10.0"/>
Expand Down
8 changes: 4 additions & 4 deletions localization/ekf_localizer/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,13 +67,13 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi

Estimated pose with covariance.

- ekf_pose_with_covariance (geometry_msgs/PoseStamped)
- ekf_biased_pose (geometry_msgs/PoseStamped)

Estimated pose without yawbias effect.
Estimated pose including the yaw bias

- ekf_pose_with_covariance_without_yawbias (geometry_msgs/PoseWithCovarianceStamped)
- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)

Estimated pose with covariance without yawbias effect.
Estimated pose with covariance including the yaw bias

- ekf_twist (geometry_msgs/TwistStamped)

Expand Down
7 changes: 3 additions & 4 deletions localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,10 +130,9 @@ class EKFLocalizer : public rclcpp::Node
//!< @brief ekf estimated yaw bias publisher
rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_yaw_bias_;
//!< @brief ekf estimated yaw bias publisher
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_no_yawbias_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_biased_pose_;
//!< @brief ekf estimated yaw bias publisher
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
pub_pose_cov_no_yawbias_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_biased_pose_cov_;
//!< @brief initial pose subscriber
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
//!< @brief measurement pose with covariance subscriber
Expand Down Expand Up @@ -203,7 +202,7 @@ class EKFLocalizer : public rclcpp::Node
std::queue<PoseInfo> current_pose_info_queue_; //!< @brief current measured pose
geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose
geometry_msgs::msg::PoseStamped
current_ekf_pose_no_yawbias_; //!< @brief current estimated pose w/o yaw bias
current_biased_ekf_pose_; //!< @brief current estimated pose without yaw bias correction
geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist
std::array<double, 36ul> current_pose_covariance_;
std::array<double, 36ul> current_twist_covariance_;
Expand Down
8 changes: 4 additions & 4 deletions localization/ekf_localizer/launch/ekf_localizer.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@
<arg name="output_odom_name" default="ekf_odom"/>
<arg name="output_pose_name" default="ekf_pose"/>
<arg name="output_pose_with_covariance_name" default="ekf_pose_with_covariance"/>
<arg name="output_pose_without_yawbias_name" default="ekf_pose_without_yawbias"/>
<arg name="output_pose_with_covariance_without_yawbias_name" default="ekf_pose_with_covariance_without_yawbias"/>
<arg name="output_biased_pose_name" default="ekf_biased_pose"/>
<arg name="output_biased_pose_with_covariance_name" default="ekf_biased_pose_with_covariance"/>
<arg name="output_twist_name" default="ekf_twist"/>
<arg name="output_twist_with_covariance_name" default="ekf_twist_with_covariance"/>

Expand Down Expand Up @@ -66,8 +66,8 @@
<remap from="ekf_odom" to="$(var output_odom_name)"/>
<remap from="ekf_pose" to="$(var output_pose_name)"/>
<remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)"/>
<remap from="ekf_pose_without_yawbias" to="$(var output_pose_without_yawbias_name)"/>
<remap from="ekf_pose_with_covariance_without_yawbias" to="$(var output_pose_with_covariance_without_yawbias_name)"/>
<remap from="ekf_biased_pose" to="$(var output_biased_pose_name)"/>
<remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)"/>
<remap from="ekf_twist" to="$(var output_twist_name)"/>
<remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)"/>
</node>
Expand Down
19 changes: 9 additions & 10 deletions localization/ekf_localizer/src/ekf_localizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,10 +89,9 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
pub_twist_cov_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
"ekf_twist_with_covariance", 1);
pub_yaw_bias_ = create_publisher<tier4_debug_msgs::msg::Float64Stamped>("estimated_yaw_bias", 1);
pub_pose_no_yawbias_ =
create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose_without_yawbias", 1);
pub_pose_cov_no_yawbias_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"ekf_pose_with_covariance_without_yawbias", 1);
pub_biased_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_biased_pose", 1);
pub_biased_pose_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"ekf_biased_pose_with_covariance", 1);
sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1));
sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
Expand Down Expand Up @@ -229,8 +228,8 @@ void EKFLocalizer::setCurrentResult()
current_ekf_pose_.pose.orientation =
tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);

current_ekf_pose_no_yawbias_ = current_ekf_pose_;
current_ekf_pose_no_yawbias_.pose.orientation =
current_biased_ekf_pose_ = current_ekf_pose_;
current_biased_ekf_pose_.pose.orientation =
tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW));

current_ekf_twist_.header.frame_id = "base_link";
Expand Down Expand Up @@ -661,7 +660,7 @@ void EKFLocalizer::publishEstimateResult()

/* publish latest pose */
pub_pose_->publish(current_ekf_pose_);
pub_pose_no_yawbias_->publish(current_ekf_pose_no_yawbias_);
pub_biased_pose_->publish(current_biased_ekf_pose_);

/* publish latest pose with covariance */
geometry_msgs::msg::PoseWithCovarianceStamped pose_cov;
Expand All @@ -679,9 +678,9 @@ void EKFLocalizer::publishEstimateResult()
pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW);
pub_pose_cov_->publish(pose_cov);

geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_no_yawbias = pose_cov;
pose_cov_no_yawbias.pose.pose = current_ekf_pose_no_yawbias_.pose;
pub_pose_cov_no_yawbias_->publish(pose_cov_no_yawbias);
geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov;
biased_pose_cov.pose.pose = current_biased_ekf_pose_.pose;
pub_biased_pose_cov_->publish(biased_pose_cov);

/* publish latest twist */
pub_twist_->publish(current_ekf_twist_);
Expand Down