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

feat(imu_corrector): changed input topic of GyroBiasEstimator from ndt_pose to ekf_odom #5374

Merged
merged 1 commit into from
Oct 24, 2023
Merged

feat(imu_corrector): changed input topic of GyroBiasEstimator from ndt_pose to ekf_odom #5374

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 2 additions & 2 deletions sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="input_imu_raw" default="imu_raw"/>
<arg name="input_pose" default="pose"/>
<arg name="input_odom" default="odom"/>
<arg name="output_gyro_bias" default="gyro_bias"/>
<arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
<arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>

<node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
<remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
<remap from="~/input/pose" to="$(var input_pose)"/>
<remap from="~/input/odom" to="$(var input_odom)"/>
<remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>
<param from="$(var gyro_bias_estimator_param_file)"/>
<param from="$(var imu_corrector_param_file)"/>
Expand Down
13 changes: 6 additions & 7 deletions sensing/imu_corrector/src/gyro_bias_estimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@ GyroBiasEstimator::GyroBiasEstimator()
imu_sub_ = create_subscription<Imu>(
"~/input/imu_raw", rclcpp::SensorDataQoS(),
[this](const Imu::ConstSharedPtr msg) { callback_imu(msg); });
pose_sub_ = create_subscription<PoseWithCovarianceStamped>(
"~/input/pose", rclcpp::SensorDataQoS(),
[this](const PoseWithCovarianceStamped::ConstSharedPtr msg) { callback_pose(msg); });
odom_sub_ = create_subscription<Odometry>(
"~/input/odom", rclcpp::SensorDataQoS(),
[this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); });
gyro_bias_pub_ = create_publisher<Vector3Stamped>("~/output/gyro_bias", rclcpp::SensorDataQoS());

auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this);
Expand Down Expand Up @@ -88,12 +88,11 @@ void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr)
}
}

void GyroBiasEstimator::callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr)
void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr)
{
// push pose_msg to queue
geometry_msgs::msg::PoseStamped pose;
pose.header = pose_msg_ptr->header;
pose.pose = pose_msg_ptr->pose.pose;
pose.header = odom_msg_ptr->header;
pose.pose = odom_msg_ptr->pose.pose;
pose_buf_.push_back(pose);
}

Expand Down
6 changes: 4 additions & 2 deletions sensing/imu_corrector/src/gyro_bias_estimator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/imu.hpp>

#include <memory>
Expand All @@ -38,14 +39,15 @@ class GyroBiasEstimator : public rclcpp::Node
using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
using Vector3Stamped = geometry_msgs::msg::Vector3Stamped;
using Vector3 = geometry_msgs::msg::Vector3;
using Odometry = nav_msgs::msg::Odometry;

public:
GyroBiasEstimator();

private:
void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr);
void callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr);
void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr);
void timer_callback();

static geometry_msgs::msg::Vector3 transform_vector3(
Expand All @@ -55,7 +57,7 @@ class GyroBiasEstimator : public rclcpp::Node
const std::string output_frame_ = "base_link";

rclcpp::Subscription<Imu>::SharedPtr imu_sub_;
rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
rclcpp::Publisher<Vector3Stamped>::SharedPtr gyro_bias_pub_;
rclcpp::TimerBase::SharedPtr timer_;

Expand Down