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(gyro_odometer): use all data #2293

Closed
wants to merge 18 commits into from
Closed

fix(gyro_odometer): use all data #2293

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
a8b98d4
feature(gyro_odometer): use all data
kminoda Nov 15, 2022
ecd7554
ci(pre-commit): autofix
pre-commit-ci[bot] Nov 15, 2022
a6d2363
drop data when one or both is empty
kminoda Nov 15, 2022
0e0f6e1
Merge branch 'feature/gyro_odometer/use_all_data' of github.com:kmino…
kminoda Nov 15, 2022
d2d1e47
now works
kminoda Nov 16, 2022
016d270
ci(pre-commit): autofix
pre-commit-ci[bot] Nov 16, 2022
bc9c599
now validation framework is the same as the previous version
kminoda Nov 16, 2022
a2f32db
resolve conflict
kminoda Nov 16, 2022
d7cf02b
ci(pre-commit): autofix
pre-commit-ci[bot] Nov 16, 2022
5759326
minor fix
kminoda Nov 16, 2022
74d38f4
reference in for
kminoda Nov 16, 2022
6ae751f
add my name in the ToDo
kminoda Nov 17, 2022
fc63b56
small fix
kminoda Nov 30, 2022
5a61a36
small fix
kminoda Nov 30, 2022
b81c169
remove unnecessary function
kminoda Nov 30, 2022
7efb244
debugged
kminoda Nov 30, 2022
fb5e471
return when IMU/odom is not subscribed
kminoda Nov 30, 2022
4d97a37
add launch param
kminoda Dec 1, 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
Prev Previous commit
Next Next commit
now works 
Signed-off-by: kminoda <koji.minoda@tier4.jp>
  • Loading branch information
@kminoda
kminoda committed Nov 16, 2022
commit d2d1e472472b91ee962fe0dc730edbbc18f1fcf8
7 changes: 3 additions & 4 deletions localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,9 @@ class GyroOdometer : public rclcpp::Node
bool getTransform(
const std::string & target_frame, const std::string & source_frame,
const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr);
void validityCheck(
const bool is_velocity_arrived, const bool is_imu_arrived,
const std::vector<TwistWithCovarianceStamped> & velocity_buffer,
const std::vector<TwistWithCovarianceStamped> & gyro_buffer) const;
void checkTimeout(
const std::vector<geometry_msgs::msg::TwistWithCovarianceStamped> & velocity_buffer,
const std::vector<geometry_msgs::msg::TwistWithCovarianceStamped> & gyro_buffer) const;

rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr vehicle_twist_with_cov_sub_;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
Expand Down
85 changes: 46 additions & 39 deletions localization/gyro_odometer/src/gyro_odometer_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,13 +35,13 @@ geometry_msgs::msg::TwistWithCovarianceStamped get_twist_from_velocity_buffer(
return twist_with_cov;
}

int n = static_cast<int>(velocity_buffer.size());
for (const auto vel : velocity_buffer) {
twist_with_cov.twist.twist.linear.x += vel.twist.twist.linear.x;
twist_with_cov.twist.covariance[0] += vel.twist.covariance[0];
}
twist_with_cov.twist.twist.linear.x /= velocity_buffer.size();
twist_with_cov.twist.covariance[0] /=
velocity_buffer.size(); // TODO: divide by appropriate value!!!!!!!!!
twist_with_cov.twist.twist.linear.x /= n;
twist_with_cov.twist.covariance[0] /= n * n; // Central limit theorem
return twist_with_cov;
}

Expand All @@ -56,6 +56,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped get_twist_from_gyro_buffer(
return twist_with_cov;
}

int n = static_cast<int>(gyro_buffer.size());
for (const auto gyro : gyro_buffer) {
twist_with_cov.twist.twist.angular.x += gyro.twist.twist.angular.x;
twist_with_cov.twist.twist.angular.y += gyro.twist.twist.angular.y;
Expand All @@ -64,13 +65,12 @@ geometry_msgs::msg::TwistWithCovarianceStamped get_twist_from_gyro_buffer(
twist_with_cov.twist.covariance[4 * 6 + 4] += gyro.twist.covariance[4 * 6 + 4];
twist_with_cov.twist.covariance[5 * 6 + 5] += gyro.twist.covariance[5 * 6 + 5];
}
twist_with_cov.twist.twist.angular.x /= gyro_buffer.size();
twist_with_cov.twist.twist.angular.y /= gyro_buffer.size();
twist_with_cov.twist.twist.angular.z /= gyro_buffer.size();
twist_with_cov.twist.covariance[3 * 6 + 3] /=
gyro_buffer.size(); // TODO: divide by appropriate value!!!!!!!!!
twist_with_cov.twist.covariance[4 * 6 + 4] /= gyro_buffer.size();
twist_with_cov.twist.covariance[5 * 6 + 5] /= gyro_buffer.size();
twist_with_cov.twist.twist.angular.x /= n;
twist_with_cov.twist.twist.angular.y /= n;
twist_with_cov.twist.twist.angular.z /= n;
twist_with_cov.twist.covariance[3 * 6 + 3] /= n * n; // Central limit theorem
twist_with_cov.twist.covariance[4 * 6 + 4] /= n * n;
twist_with_cov.twist.covariance[5 * 6 + 5] /= n * n;
return twist_with_cov;
}

Expand Down Expand Up @@ -126,15 +126,36 @@ GyroOdometer::~GyroOdometer() {}

void GyroOdometer::timerCallback()
{
std::string error_msg;
if (!is_velocity_arrived_) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed");
}
if (!is_imu_arrived_) {
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed");
}
if (vel_buffer_.empty() || gyro_buffer_.empty()) {
vel_buffer_.clear();
gyro_buffer_.clear();
return;
}
try {
validityCheck(is_velocity_arrived_, is_imu_arrived_, vel_buffer_, gyro_buffer_);
} catch (const std::exception & e) {
RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, e.what());

const double velocity_dt =
std::abs((this->now() - vel_buffer_.front().header.stamp).seconds());
const double imu_dt = std::abs((this->now() - gyro_buffer_.front().header.stamp).seconds());

if (velocity_dt > message_timeout_sec_) {
std::string error_msg = fmt::format(
"Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", velocity_dt,
message_timeout_sec_);
RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str());
vel_buffer_.clear();
gyro_buffer_.clear();
return;
}
if (imu_dt > message_timeout_sec_) {
std::string error_msg = fmt::format(
"Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_);
RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str());
vel_buffer_.clear();
gyro_buffer_.clear();
return;
Expand Down Expand Up @@ -254,36 +275,22 @@ bool GyroOdometer::getTransform(
return true;
}

void GyroOdometer::validityCheck(
const bool is_velocity_arrived, const bool is_imu_arrived,
void GyroOdometer::checkTimeout(
YamatoAndo marked this conversation as resolved.
Show resolved Hide resolved
const std::vector<geometry_msgs::msg::TwistWithCovarianceStamped> & velocity_buffer,
const std::vector<geometry_msgs::msg::TwistWithCovarianceStamped> & gyro_buffer) const
{
std::string error_msg;
if (!is_velocity_arrived) {
error_msg = "Twist msg is not subscribed";
const double velocity_dt =
std::abs((this->now() - velocity_buffer.front().header.stamp).seconds());
if (velocity_dt > message_timeout_sec_) {
std::string error_msg = fmt::format(
"Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", velocity_dt,
message_timeout_sec_);
throw std::domain_error(error_msg);
}
if (!is_imu_arrived) {
error_msg = "Imu msg is not subscribed";
const double imu_dt = std::abs((this->now() - gyro_buffer.front().header.stamp).seconds());
if (imu_dt > message_timeout_sec_) {
std::string error_msg = fmt::format(
"Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_);
throw std::domain_error(error_msg);
}
if (!velocity_buffer.empty()) {
const double velocity_dt =
std::abs((this->now() - velocity_buffer.front().header.stamp).seconds());
if (velocity_dt > message_timeout_sec_) {
error_msg = fmt::format(
"Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", velocity_dt,
message_timeout_sec_);
throw std::domain_error(error_msg);
}
}
if (!gyro_buffer.empty()) {
const double imu_dt = std::abs((this->now() - gyro_buffer.front().header.stamp).seconds());
if (imu_dt > message_timeout_sec_) {
error_msg = fmt::format(
"Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_);
throw std::domain_error(error_msg);
}
}
}