Skip to content

Commit

Permalink
Drop out of sequence wheel odometry messages
Browse files Browse the repository at this point in the history
  • Loading branch information
@smauq
smauq committed Dec 10, 2019
1 parent 1f09fb2 commit ace4cc2
Show file tree
Hide file tree
Showing 9 changed files with 75 additions and 14 deletions.
1 change: 1 addition & 0 deletions applications/maplab_node/include/maplab-node/datasource-rosbag.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ class DataSourceRosbag : public DataSource {

int64_t last_imu_timestamp_ns_;
std::vector<int64_t> last_image_timestamp_ns_;
int64_t last_wheel_odometry_timestamp_ns_;

};

Expand Down
1 change: 1 addition & 0 deletions applications/maplab_node/include/maplab-node/datasource-rostopic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,7 @@ class DataSourceRostopic : public DataSource {

int64_t last_imu_timestamp_ns_;
std::vector<int64_t> last_image_timestamp_ns_;
int64_t last_wheel_odometry_timestamp_ns_;
};

} // namespace maplab
Expand Down
30 changes: 21 additions & 9 deletions applications/maplab_node/src/datasource-rosbag.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,8 @@ DataSourceRosbag::DataSourceRosbag(
all_data_streamed_(false),
rosbag_path_filename_(rosbag_path_filename),
ros_topics_(ros_topics),
last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
last_imu_timestamp_ns_(aslam::time::getInvalidTime()),
last_wheel_odometry_timestamp_ns_(aslam::time::getInvalidTime()) {
const uint8_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
if (num_cameras > 0u) {
last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
Expand Down Expand Up @@ -350,14 +351,25 @@ void DataSourceRosbag::streamingWorker() {
vi_map::WheelOdometryMeasurement::Ptr wheel_odometry_measurement =
convertRosOdometryToMaplabWheelOdometry(
wheel_odometry_msg, sensor_id);
if (!wheel_odometry_measurement) {
LOG(ERROR) << "Received INVALID wheel odometry constraint!";
return;
} else {
// Shift timestamps to start at 0.
if (!FLAGS_zero_initial_timestamps ||
shiftByFirstTimestamp(
wheel_odometry_measurement->getTimestampNanosecondsMutable())) {
CHECK(wheel_odometry_measurement);

// Shift timestamps to start at 0.
if (!FLAGS_zero_initial_timestamps ||
shiftByFirstTimestamp(
wheel_odometry_measurement->getTimestampNanosecondsMutable())) {
// Check for strictly increasing wheel odometry timestamps.
if (aslam::time::isValidTime(last_wheel_odometry_timestamp_ns_) &&
last_wheel_odometry_timestamp_ns_ >=
wheel_odometry_measurement->getTimestampNanoseconds()) {
LOG(WARNING) << "[MaplabNode-DataSource] Wheel odometry message is "
<< "not strictly increasing! Current timestamp: "
<< wheel_odometry_measurement->getTimestampNanoseconds()
<< "ns vs last timestamp: "
<< last_wheel_odometry_timestamp_ns_ << "ns.";
} else {
last_wheel_odometry_timestamp_ns_ =
wheel_odometry_measurement->getTimestampNanoseconds();

VLOG(3) << "Publish wheel odometry constraint...";
invokeWheelOdometryConstraintCallbacks(wheel_odometry_measurement);
}
Expand Down
18 changes: 17 additions & 1 deletion applications/maplab_node/src/datasource-rostopic.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,8 @@ DataSourceRostopic::DataSourceRostopic(
shutdown_requested_(false),
ros_topics_(ros_topics),
image_transport_(node_handle_),
last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
last_imu_timestamp_ns_(aslam::time::getInvalidTime()),
last_wheel_odometry_timestamp_ns_(aslam::time::getInvalidTime()) {
const uint8_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
if (num_cameras > 0u) {
last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
Expand Down Expand Up @@ -434,6 +435,21 @@ void DataSourceRostopic::wheelOdometryConstraintCallback(
if (!FLAGS_zero_initial_timestamps ||
(shiftByFirstTimestamp(
wheel_odometry_measurement->getTimestampNanosecondsMutable()))) {
// Check for strictly increasing wheel odometry timestamps.
if (aslam::time::isValidTime(last_wheel_odometry_timestamp_ns_) &&
last_wheel_odometry_timestamp_ns_ >=
wheel_odometry_measurement->getTimestampNanoseconds()) {
LOG(WARNING) << "[MaplabNode-DataSource] Wheel odometry message is "
<< "not strictly increasing! Current timestamp: "
<< wheel_odometry_measurement->getTimestampNanoseconds()
<< "ns vs last timestamp: "
<< last_wheel_odometry_timestamp_ns_ << "ns.";
return;
} else {
last_wheel_odometry_timestamp_ns_ =
wheel_odometry_measurement->getTimestampNanoseconds();
}

invokeWheelOdometryConstraintCallbacks(wheel_odometry_measurement);
}
}
Expand Down
1 change: 1 addition & 0 deletions applications/rovioli/include/rovioli/datasource-rosbag.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ class DataSourceRosbag : public DataSource {

int64_t last_imu_timestamp_ns_;
std::vector<int64_t> last_image_timestamp_ns_;
int64_t last_odometry_timestamp_ns_;
};

} // namespace rovioli
Expand Down
1 change: 1 addition & 0 deletions applications/rovioli/include/rovioli/datasource-rostopic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ class DataSourceRostopic : public DataSource {

int64_t last_imu_timestamp_ns_;
std::vector<int64_t> last_image_timestamp_ns_;
int64_t last_odometry_timestamp_ns_;
};

} // namespace rovioli
Expand Down
18 changes: 16 additions & 2 deletions applications/rovioli/src/datasource-rosbag.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,8 @@ DataSourceRosbag::DataSourceRosbag(
all_data_streamed_(false),
rosbag_path_filename_(rosbag_path_filename),
ros_topics_(ros_topics),
last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
last_imu_timestamp_ns_(aslam::time::getInvalidTime()),
last_odometry_timestamp_ns_(aslam::time::getInvalidTime()) {
const uint8_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
if (num_cameras > 0u) {
last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
Expand Down Expand Up @@ -232,7 +233,20 @@ void DataSourceRosbag::streamingWorker() {
// Shift timestamps to start at 0.
if (!FLAGS_rovioli_zero_initial_timestamps ||
shiftByFirstTimestamp(&(odometry_measurement->timestamp))) {
invokeOdometryCallbacks(odometry_measurement);
// Check for strictly increasing wheel odometry timestamps.
if (aslam::time::isValidTime(last_odometry_timestamp_ns_) &&
last_odometry_timestamp_ns_ >= odometry_measurement->timestamp) {
LOG(WARNING) << "[MaplabNode-DataSource] Wheel odometry message is "
<< "not strictly increasing! Current timestamp: "
<< odometry_measurement->timestamp
<< "ns vs last timestamp: "
<< last_odometry_timestamp_ns_ << "ns.";
} else {
last_odometry_timestamp_ns_ = odometry_measurement->timestamp;

VLOG(3) << "Publish odometry measurement...";
invokeOdometryCallbacks(odometry_measurement);
}
}
}

Expand Down
15 changes: 14 additions & 1 deletion applications/rovioli/src/datasource-rostopic.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,8 @@ DataSourceRostopic::DataSourceRostopic(
: shutdown_requested_(false),
ros_topics_(ros_topics),
image_transport_(node_handle_),
last_imu_timestamp_ns_(aslam::time::getInvalidTime()) {
last_imu_timestamp_ns_(aslam::time::getInvalidTime()),
last_odometry_timestamp_ns_(aslam::time::getInvalidTime()) {
const uint8_t num_cameras = ros_topics_.camera_topic_cam_index_map.size();
if (num_cameras > 0u) {
last_image_timestamp_ns_.resize(num_cameras, aslam::time::getInvalidTime());
Expand Down Expand Up @@ -155,6 +156,18 @@ void DataSourceRostopic::odometryMeasurementCallback(
// Shift timestamps to start at 0.
if (!FLAGS_rovioli_zero_initial_timestamps ||
shiftByFirstTimestamp(&(odometry_measurement->timestamp))) {
// Check for strictly increasing wheel odometry timestamps.
if (aslam::time::isValidTime(last_odometry_timestamp_ns_) &&
last_odometry_timestamp_ns_ >= odometry_measurement->timestamp) {
LOG(WARNING) << "[MaplabNode-DataSource] Wheel odometry message is "
<< "not strictly increasing! Current timestamp: "
<< odometry_measurement->timestamp
<< "ns vs last timestamp: " << last_odometry_timestamp_ns_
<< "ns.";
} else {
last_odometry_timestamp_ns_ = odometry_measurement->timestamp;
}

invokeOdometryCallbacks(odometry_measurement);
}
}
Expand Down
4 changes: 3 additions & 1 deletion common/vio-common/include/vio-common/vio-types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,11 +132,13 @@ enum class UpdateType { kInvalid, kNormalUpdate, kZeroVelocityUpdate };
struct OdometryMeasurement {
MAPLAB_POINTER_TYPEDEFS(OdometryMeasurement);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
OdometryMeasurement() = default;

int64_t timestamp;
// Velocities in odometry sensor frame.
Eigen::Vector3d velocity_linear_O;
Eigen::Vector3d velocity_angular_O;

OdometryMeasurement() : timestamp(aslam::time::getInvalidTime()) {}
};

/// A data structure containing a VisualNFrame
Expand Down

0 comments on commit ace4cc2

Please sign in to comment.