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(motion_utils): check if the buffer is empty #3883

Merged
merged 2 commits into from
Jun 2, 2023
Merged

fix(motion_utils): check if the buffer is empty #3883

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a85978a
fix(motion_utils): check if the buffer is empty
TomohitoAndo Jun 1, 2023
5869ac9
add test to check if the old data is discarded
TomohitoAndo Jun 1, 2023
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 common/motion_utils/src/vehicle/vehicle_state_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ void VehicleStopCheckerBase::addTwist(const TwistStamped & twist)
twist_buffer_.push_front(twist);

const auto now = clock_->now();
while (true) {
while (!twist_buffer_.empty()) {
// Check oldest data time
const auto time_diff = now - twist_buffer_.back().header.stamp;

Expand Down
54 changes: 54 additions & 0 deletions common/motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,29 @@ class PubManager : public rclcpp::Node
rclcpp::WallRate(10).sleep();
}
}

void publishOldOdometry(const double publish_duration)
{
const auto start_time = this->now();
while (true) {
const auto now = this->now();

const auto time_diff = now - start_time;
if (publish_duration < time_diff.seconds()) {
break;
}

Odometry odometry;
odometry.header.stamp = now - rclcpp::Duration(15, 0); // 15 seconds old data
odometry.pose.pose.position = createPoint(0.0, 0.0, 0.0);
odometry.pose.pose.orientation = createQuaternion(0.0, 0.0, 0.0, 1.0);
odometry.twist.twist.linear = createTranslation(0.0, 0.0, 0.0);
odometry.twist.twist.angular = createTranslation(0.0, 0.0, 0.0);
this->pub_odom_->publish(odometry);

rclcpp::WallRate(10).sleep();
}
}
};

TEST(vehicle_stop_checker, isVehicleStopped)
Expand Down Expand Up @@ -226,6 +249,37 @@ TEST(vehicle_stop_checker, isVehicleStopped)
checker.reset();
manager.reset();
}

// check if the old data will be discarded
{
auto checker = std::make_shared<CheckerNode>();
auto manager = std::make_shared<PubManager>();
EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";

EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped());

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(checker);
executor.add_node(manager);

std::thread spin_thread =
std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor));

manager->publishOldOdometry(ODOMETRY_HISTORY_500_MS);

EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped());
EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS));
EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS));
EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS));
EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS));
EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS));

executor.cancel();
spin_thread.join();
checker.reset();
manager.reset();
}
}

TEST(vehicle_arrival_checker, isVehicleStopped)
Expand Down