Skip to content

Commit

Permalink
track_odometry: synchronize Odometry and IMU (#363)
Browse files Browse the repository at this point in the history 
* Add delay and jitter to track_odometry test

* Add small difference to the timestamp of each topic

* Synchronize Odometry and IMU by ApproximateTime policy
  • Loading branch information
@at-wat
at-wat authored Jul 31, 2019
1 parent 85dd87e commit fbee5e0
Show file tree
Hide file tree
Showing 4 changed files with 103 additions and 33 deletions.
1 change: 1 addition & 0 deletions track_odometry/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ set(CATKIN_DEPENDS
cmake_modules

geometry_msgs
message_filters
nav_msgs
sensor_msgs
std_msgs
Expand Down
1 change: 1 addition & 0 deletions track_odometry/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
<test_depend>rosunit</test_depend>

<depend>geometry_msgs</depend>
<depend>message_filters</depend>
<depend>nav_msgs</depend>
<depend>sensor_msgs</depend>
<depend>std_msgs</depend>
Expand Down
77 changes: 58 additions & 19 deletions track_odometry/src/track_odometry.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (c) 2014-2017, the neonavigation authors
* Copyright (c) 2014-2019, the neonavigation authors
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
Expand Down Expand Up @@ -33,6 +33,10 @@
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
Expand Down Expand Up @@ -79,10 +83,17 @@ geometry_msgs::Vector3 toVector3(const Eigen::Vector3f& a)
class TrackOdometryNode
{
private:
using SyncPolicy =
message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry, sensor_msgs::Imu>;

ros::NodeHandle nh_;
ros::NodeHandle pnh_;
ros::Subscriber sub_odom_;
ros::Subscriber sub_imu_;

ros::Subscriber sub_imu_raw_;
std::shared_ptr<message_filters::Subscriber<nav_msgs::Odometry>> sub_odom_;
std::shared_ptr<message_filters::Subscriber<sensor_msgs::Imu>> sub_imu_;
std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;

ros::Subscriber sub_reset_z_;
ros::Publisher pub_odom_;
tf2_ros::Buffer tf_buffer_;
Expand Down Expand Up @@ -119,7 +130,16 @@ class TrackOdometryNode
{
odom_prev_.pose.pose.position.z = msg->data;
}
void cbImu(const sensor_msgs::Imu::Ptr& msg)
void cbOdomImu(const nav_msgs::Odometry::ConstPtr& odom_msg, const sensor_msgs::Imu::ConstPtr& imu_msg)
{
ROS_DEBUG(
"Synchronized timestamp: odom %0.3f, imu %0.3f",
odom_msg->header.stamp.toSec(),
imu_msg->header.stamp.toSec());
cbImu(imu_msg);
cbOdom(odom_msg);
}
void cbImu(const sensor_msgs::Imu::ConstPtr& msg)
{
if (base_link_id_.size() == 0)
{
Expand Down Expand Up @@ -181,7 +201,7 @@ class TrackOdometryNode
return;
}
}
void cbOdom(const nav_msgs::Odometry::Ptr& msg)
void cbOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
nav_msgs::Odometry odom = *msg;
if (has_odom_)
Expand Down Expand Up @@ -275,27 +295,46 @@ class TrackOdometryNode
, tf_listener_(tf_buffer_)
{
neonavigation_common::compat::checkCompatMode();
pnh_.param("without_odom", without_odom_, false);
if (!without_odom_)
sub_odom_ = nh_.subscribe("odom_raw", 64, &TrackOdometryNode::cbOdom, this);
sub_imu_ = neonavigation_common::compat::subscribe(
nh_, "imu/data",
nh_, "imu", 64, &TrackOdometryNode::cbImu, this);
sub_reset_z_ = neonavigation_common::compat::subscribe(
nh_, "reset_odometry_z",
pnh_, "reset_z", 1, &TrackOdometryNode::cbResetZ, this);
pub_odom_ = nh_.advertise<nav_msgs::Odometry>("odom", 8);

if (!without_odom_)
pnh_.param("without_odom", without_odom_, false);
if (without_odom_)
{
pnh_.param("base_link_id", base_link_id_overwrite_, std::string(""));
sub_imu_raw_ = neonavigation_common::compat::subscribe(
nh_, "imu/data",
nh_, "imu", 64, &TrackOdometryNode::cbImu, this);
pnh_.param("base_link_id", base_link_id_, std::string("base_link"));
pnh_.param("odom_id", odom_id_, std::string("odom"));
}
else
{
pnh_.param("base_link_id", base_link_id_, std::string("base_link"));
pnh_.param("odom_id", odom_id_, std::string("odom"));
sub_odom_.reset(
new message_filters::Subscriber<nav_msgs::Odometry>(nh_, "odom_raw", 1));
if (neonavigation_common::compat::getCompat() == neonavigation_common::compat::current_level)
{
sub_imu_.reset(
new message_filters::Subscriber<sensor_msgs::Imu>(nh_, "imu/data", 1));
}
else
{
sub_imu_.reset(
new message_filters::Subscriber<sensor_msgs::Imu>(nh_, "imu", 1));
}

int sync_window;
pnh_.param("sync_window", sync_window, 50);
sync_.reset(
new message_filters::Synchronizer<SyncPolicy>(
SyncPolicy(sync_window), *sub_odom_, *sub_imu_));
sync_->registerCallback(boost::bind(&TrackOdometryNode::cbOdomImu, this, _1, _2));

pnh_.param("base_link_id", base_link_id_overwrite_, std::string(""));
}

sub_reset_z_ = neonavigation_common::compat::subscribe(
nh_, "reset_odometry_z",
pnh_, "reset_z", 1, &TrackOdometryNode::cbResetZ, this);
pub_odom_ = nh_.advertise<nav_msgs::Odometry>("odom", 8);

if (pnh_.hasParam("z_filter"))
{
z_filter_timeconst_ = -1.0;
Expand Down
57 changes: 43 additions & 14 deletions track_odometry/test/src/test_track_odometry.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (c) 2018, the neonavigation authors
* Copyright (c) 2018-2019, the neonavigation authors
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
Expand Down Expand Up @@ -28,6 +28,7 @@
*/

#include <string>
#include <vector>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
Expand All @@ -39,6 +40,9 @@

class TrackOdometryTest : public ::testing::TestWithParam<const char*>
{
protected:
std::vector<nav_msgs::Odometry> odom_msg_buffer_;

public:
void initializeNode(const std::string& ns)
{
Expand All @@ -47,7 +51,7 @@ class TrackOdometryTest : public ::testing::TestWithParam<const char*>
pub_imu_ = nh.advertise<sensor_msgs::Imu>("imu/data", 10);
sub_odom_ = nh.subscribe("odom", 10, &TrackOdometryTest::cbOdom, this);
}
void initializeTrackOdometry(
bool initializeTrackOdometry(
nav_msgs::Odometry& odom_raw,
sensor_msgs::Imu& imu)
{
Expand All @@ -56,14 +60,16 @@ class TrackOdometryTest : public ::testing::TestWithParam<const char*>
odom_ = nullptr;
for (int i = 0; i < 100 && ros::ok(); ++i)
{
imu.header.stamp = odom_raw.header.stamp = ros::Time::now();
odom_raw.header.stamp = ros::Time::now();
imu.header.stamp = odom_raw.header.stamp + ros::Duration(0.0001);
pub_odom_.publish(odom_raw);
pub_imu_.publish(imu);
rate.sleep();
ros::spinOnce();
if (odom_ && i > 50)
break;
}
return static_cast<bool>(odom_);
}
bool run(
nav_msgs::Odometry& odom_raw,
Expand All @@ -73,7 +79,6 @@ class TrackOdometryTest : public ::testing::TestWithParam<const char*>
ros::Rate rate(1.0 / dt);
int cnt(0);

imu.header.stamp = odom_raw.header.stamp = ros::Time::now();
while (ros::ok())
{
tf2::Quaternion quat_odom;
Expand All @@ -100,6 +105,7 @@ class TrackOdometryTest : public ::testing::TestWithParam<const char*>
if (++cnt >= steps)
break;
}
flushOdomMsgs();
return ros::ok();
}
void stepAndPublish(
Expand All @@ -109,8 +115,25 @@ class TrackOdometryTest : public ::testing::TestWithParam<const char*>
{
odom_raw.header.stamp += ros::Duration(dt);
imu.header.stamp += ros::Duration(dt);
pub_odom_.publish(odom_raw);
pub_imu_.publish(imu);

// Buffer odom message to add delay and jitter.
// Send odometry in half rate of IMU.
++odom_cnt_;
if (odom_cnt_ % 2 == 0)
odom_msg_buffer_.push_back(odom_raw);
if (odom_msg_buffer_.size() > 10)
{
flushOdomMsgs();
}
}
void flushOdomMsgs()
{
for (nav_msgs::Odometry& o : odom_msg_buffer_)
{
pub_odom_.publish(o);
}
odom_msg_buffer_.clear();
}
void waitAndSpinOnce()
{
Expand All @@ -123,6 +146,7 @@ class TrackOdometryTest : public ::testing::TestWithParam<const char*>
ros::Publisher pub_imu_;
ros::Subscriber sub_odom_;
nav_msgs::Odometry::ConstPtr odom_;
size_t odom_cnt_;

void cbOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
Expand All @@ -134,8 +158,8 @@ TEST_F(TrackOdometryTest, OdomImuFusion)
{
initializeNode("");

const float dt = 0.02;
const int steps = 100;
const float dt = 0.01;
const int steps = 200;

nav_msgs::Odometry odom_raw;
odom_raw.header.frame_id = "odom";
Expand All @@ -146,14 +170,15 @@ TEST_F(TrackOdometryTest, OdomImuFusion)
imu.orientation.w = 1;
imu.linear_acceleration.z = 9.8;

initializeTrackOdometry(odom_raw, imu);
ASSERT_TRUE(initializeTrackOdometry(odom_raw, imu));

// Go forward for 1m
odom_raw.twist.twist.linear.x = 0.5;
ASSERT_TRUE(run(odom_raw, imu, dt, steps));

odom_raw.twist.twist.linear.x = 0.0;
stepAndPublish(odom_raw, imu, dt);
flushOdomMsgs();

waitAndSpinOnce();
ASSERT_NEAR(odom_->pose.pose.position.x, 1.0, 1e-3);
Expand All @@ -170,6 +195,7 @@ TEST_F(TrackOdometryTest, OdomImuFusion)
odom_raw.twist.twist.angular.z = 0;
imu.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), M_PI / 2));
stepAndPublish(odom_raw, imu, dt);
flushOdomMsgs();

waitAndSpinOnce();
ASSERT_NEAR(odom_->pose.pose.position.x, 1.0, 1e-2);
Expand All @@ -183,6 +209,7 @@ TEST_F(TrackOdometryTest, OdomImuFusion)

odom_raw.twist.twist.linear.x = 0.0;
stepAndPublish(odom_raw, imu, dt);
flushOdomMsgs();

waitAndSpinOnce();
ASSERT_NEAR(odom_->pose.pose.position.x, 1.0, 5e-2);
Expand All @@ -196,8 +223,8 @@ TEST_P(TrackOdometryTest, ZFilterOff)
const std::string ns_postfix(GetParam());
initializeNode("no_z_filter" + ns_postfix);

const float dt = 0.02;
const int steps = 100;
const float dt = 0.01;
const int steps = 200;

nav_msgs::Odometry odom_raw;
odom_raw.header.frame_id = "odom";
Expand All @@ -209,14 +236,15 @@ TEST_P(TrackOdometryTest, ZFilterOff)
imu.orientation.w = cos(-M_PI / 4);
imu.linear_acceleration.x = -9.8;

initializeTrackOdometry(odom_raw, imu);
ASSERT_TRUE(initializeTrackOdometry(odom_raw, imu));

// Go forward for 1m
odom_raw.twist.twist.linear.x = 0.5;
ASSERT_TRUE(run(odom_raw, imu, dt, steps));

odom_raw.twist.twist.linear.x = 0.0;
stepAndPublish(odom_raw, imu, dt);
flushOdomMsgs();

waitAndSpinOnce();
ASSERT_NEAR(odom_->pose.pose.position.x, 0.0, 1e-3);
Expand All @@ -229,8 +257,8 @@ TEST_P(TrackOdometryTest, ZFilterOn)
const std::string ns_postfix(GetParam());
initializeNode("z_filter" + ns_postfix);

const float dt = 0.02;
const int steps = 100;
const float dt = 0.01;
const int steps = 200;

nav_msgs::Odometry odom_raw;
odom_raw.header.frame_id = "odom";
Expand All @@ -242,14 +270,15 @@ TEST_P(TrackOdometryTest, ZFilterOn)
imu.orientation.w = cos(-M_PI / 4);
imu.linear_acceleration.x = -9.8;

initializeTrackOdometry(odom_raw, imu);
ASSERT_TRUE(initializeTrackOdometry(odom_raw, imu));

// Go forward for 1m
odom_raw.twist.twist.linear.x = 0.5;
ASSERT_TRUE(run(odom_raw, imu, dt, steps));

odom_raw.twist.twist.linear.x = 0.0;
stepAndPublish(odom_raw, imu, dt);
flushOdomMsgs();

waitAndSpinOnce();
ASSERT_NEAR(odom_->pose.pose.position.z, 1.0 - 1.0 / M_E, 5e-2);
Expand Down

0 comments on commit fbee5e0

Please sign in to comment.