diff --git a/cartographer/internal/mapping/test_helpers.cc b/cartographer/internal/mapping/test_helpers.cc index 6825871..6b31b6e 100644 --- a/cartographer/internal/mapping/test_helpers.cc +++ b/cartographer/internal/mapping/test_helpers.cc @@ -20,6 +20,7 @@ #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" +#include "cartographer/sensor/timed_point_cloud_data.h" namespace cartographer { namespace mapping { @@ -36,6 +37,34 @@ ResolveLuaParameters(const std::string& lua_code) { lua_code, std::move(file_resolver)); } +std::vector +GenerateFakeRangeMeasurements(double travel_distance, double duration, + double time_step) { + std::vector measurements; + cartographer::sensor::TimedPointCloud point_cloud; + for (double angle = 0.; angle < M_PI; angle += 0.01) { + constexpr double kRadius = 5; + point_cloud.emplace_back(kRadius * std::cos(angle), + kRadius * std::sin(angle), 0., 0.); + } + const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized(); + const Eigen::Vector3f kVelocity = travel_distance / duration * kDirection; + for (double elapsed_time = 0.; elapsed_time < duration; + elapsed_time += time_step) { + cartographer::common::Time time = + cartographer::common::FromUniversal(123) + + cartographer::common::FromSeconds(elapsed_time); + cartographer::transform::Rigid3f pose = + cartographer::transform::Rigid3f::Translation(elapsed_time * kVelocity); + cartographer::sensor::TimedPointCloud ranges = + cartographer::sensor::TransformTimedPointCloud(point_cloud, + pose.inverse()); + measurements.emplace_back(cartographer::sensor::TimedPointCloudData{ + time, Eigen::Vector3f::Zero(), ranges}); + } + return measurements; +} + } // namespace test } // namespace mapping } // namespace cartographer diff --git a/cartographer/internal/mapping/test_helpers.h b/cartographer/internal/mapping/test_helpers.h index db7b4a8..909e80a 100644 --- a/cartographer/internal/mapping/test_helpers.h +++ b/cartographer/internal/mapping/test_helpers.h @@ -20,6 +20,7 @@ #include #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/sensor/timed_point_cloud_data.h" namespace cartographer { namespace mapping { @@ -28,6 +29,10 @@ namespace test { std::unique_ptr<::cartographer::common::LuaParameterDictionary> ResolveLuaParameters(const std::string& lua_code); +std::vector +GenerateFakeRangeMeasurements(double travel_distance, double duration, + double time_step); + } // namespace test } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 0b0a754..465f373 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -35,30 +35,6 @@ constexpr double kDuration = 4.; // Seconds. constexpr double kTimeStep = 0.1; // Seconds. constexpr double kTravelDistance = 1.2; // Meters. -std::vector GenerateFakeRangeMeasurements() { - std::vector measurements; - sensor::TimedPointCloud point_cloud; - for (double angle = 0.; angle < M_PI; angle += 0.01) { - constexpr double kRadius = 5; - point_cloud.emplace_back(kRadius * std::cos(angle), - kRadius * std::sin(angle), 0., 0.); - } - const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized(); - const Eigen::Vector3f kVelocity = kTravelDistance / kDuration * kDirection; - for (double elapsed_time = 0.; elapsed_time < kDuration; - elapsed_time += kTimeStep) { - common::Time time = - common::FromUniversal(123) + common::FromSeconds(elapsed_time); - transform::Rigid3f pose = - transform::Rigid3f::Translation(elapsed_time * kVelocity); - sensor::TimedPointCloud ranges = - sensor::TransformTimedPointCloud(point_cloud, pose.inverse()); - measurements.emplace_back( - sensor::TimedPointCloudData{time, Eigen::Vector3f::Zero(), ranges}); - } - return measurements; -} - class MapBuilderTest : public ::testing::Test { protected: void SetUp() override { @@ -153,7 +129,8 @@ TEST_F(MapBuilderTest, LocalSlam2D) { GetLocalSlamResultCallback()); TrajectoryBuilderInterface* trajectory_builder = map_builder_->GetTrajectoryBuilder(trajectory_id); - const auto measurements = GenerateFakeRangeMeasurements(); + const auto measurements = test::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); for (const auto& measurement : measurements) { trajectory_builder->AddSensorData(kRangeSensorId, measurement); } @@ -177,7 +154,8 @@ TEST_F(MapBuilderTest, LocalSlam3D) { GetLocalSlamResultCallback()); TrajectoryBuilderInterface* trajectory_builder = map_builder_->GetTrajectoryBuilder(trajectory_id); - const auto measurements = GenerateFakeRangeMeasurements(); + const auto measurements = test::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); for (const auto& measurement : measurements) { trajectory_builder->AddSensorData(kRangeSensorId, measurement); trajectory_builder->AddSensorData( @@ -204,7 +182,8 @@ TEST_F(MapBuilderTest, GlobalSlam2D) { GetLocalSlamResultCallback()); TrajectoryBuilderInterface* trajectory_builder = map_builder_->GetTrajectoryBuilder(trajectory_id); - const auto measurements = GenerateFakeRangeMeasurements(); + const auto measurements = test::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); for (const auto& measurement : measurements) { trajectory_builder->AddSensorData(kRangeSensorId, measurement); } diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc index 1166838..3668a80 100644 --- a/cartographer_grpc/client_server_test.cc +++ b/cartographer_grpc/client_server_test.cc @@ -14,6 +14,9 @@ * limitations under the License. */ +#include +#include + #include "cartographer/internal/mapping/test_helpers.h" #include "cartographer_grpc/map_builder_server.h" #include "cartographer_grpc/map_builder_server_options.h" @@ -32,6 +35,10 @@ namespace cartographer_grpc { namespace { constexpr char kSensorId[] = "sensor"; +constexpr char kRangeSensorId[] = "range"; +constexpr double kDuration = 4.; // Seconds. +constexpr double kTimeStep = 0.1; // Seconds. +constexpr double kTravelDistance = 1.2; // Meters. class MockMapBuilder : public cartographer::mapping::MapBuilderInterface { public: @@ -54,6 +61,24 @@ class MockMapBuilder : public cartographer::mapping::MapBuilderInterface { MOCK_METHOD0(pose_graph, PoseGraphInterface*()); }; +class MockTrajectoryBuilder + : public cartographer::mapping::TrajectoryBuilderInterface { + public: + MockTrajectoryBuilder() = default; + ~MockTrajectoryBuilder() override = default; + + MOCK_METHOD2(AddSensorData, + void(const std::string&, + const cartographer::sensor::TimedPointCloudData&)); + MOCK_METHOD2(AddSensorData, + void(const std::string&, const cartographer::sensor::ImuData&)); + MOCK_METHOD2(AddSensorData, void(const std::string&, + const cartographer::sensor::OdometryData&)); + MOCK_METHOD2(AddSensorData, + void(const std::string&, + const cartographer::sensor::FixedFramePoseData&)); +}; + class ClientServerTest : public ::testing::Test { protected: void SetUp() override { @@ -76,6 +101,8 @@ class ClientServerTest : public ::testing::Test { CreateMapBuilderServerOptions(map_builder_server_parameters.get()); const std::string kTrajectoryBuilderLua = R"text( include "trajectory_builder.lua" + TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false + TRAJECTORY_BUILDER.trajectory_builder_2d.submaps.num_range_data = 4 return TRAJECTORY_BUILDER)text"; auto trajectory_builder_parameters = cartographer::mapping::test::ResolveLuaParameters( @@ -83,6 +110,16 @@ class ClientServerTest : public ::testing::Test { trajectory_builder_options_ = cartographer::mapping::CreateTrajectoryBuilderOptions( trajectory_builder_parameters.get()); + local_slam_result_callback_ = + [this](int, cartographer::common::Time, + cartographer::transform::Rigid3d local_pose, + cartographer::sensor::RangeData, + std::unique_ptr) { + std::unique_lock lock(local_slam_result_mutex_); + local_slam_result_poses_.push_back(local_pose); + lock.unlock(); + local_slam_result_condition_.notify_all(); + }; } void InitializeRealServer() { @@ -99,6 +136,8 @@ class ClientServerTest : public ::testing::Test { server_ = cartographer::common::make_unique( map_builder_server_options_, std::move(mock_map_builder)); EXPECT_TRUE(server_ != nullptr); + mock_trajectory_builder_ = + cartographer::common::make_unique(); } void InitializeStub() { @@ -107,12 +146,24 @@ class ClientServerTest : public ::testing::Test { EXPECT_TRUE(stub_ != nullptr); } + void WaitForLocalSlamResults(size_t size) { + std::unique_lock lock(local_slam_result_mutex_); + local_slam_result_condition_.wait( + lock, [&] { return local_slam_result_poses_.size() >= size; }); + } + proto::MapBuilderServerOptions map_builder_server_options_; MockMapBuilder* mock_map_builder_; + std::unique_ptr mock_trajectory_builder_; cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder_options_; std::unique_ptr server_; std::unique_ptr stub_; + TrajectoryBuilderInterface::LocalSlamResultCallback + local_slam_result_callback_; + std::condition_variable local_slam_result_condition_; + std::mutex local_slam_result_mutex_; + std::vector local_slam_result_poses_; }; TEST_F(ClientServerTest, StartAndStopServer) { @@ -150,5 +201,79 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) { server_->Shutdown(); } +TEST_F(ClientServerTest, AddSensorData) { + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->set_use_imu_data(true); + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = stub_->AddTrajectoryBuilder( + {kSensorId}, trajectory_builder_options_, nullptr); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + cartographer::sensor::ImuData imu_data{ + cartographer::common::FromUniversal(42), Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}; + trajectory_stub->AddSensorData(kSensorId, imu_data); + stub_->FinishTrajectory(trajectory_id); + server_->Shutdown(); +} + +TEST_F(ClientServerTest, AddSensorDataWithMock) { + InitializeServerWithMockMapBuilder(); + server_->Start(); + InitializeStub(); + std::unordered_set expected_sensor_ids = {kSensorId}; + EXPECT_CALL( + *mock_map_builder_, + AddTrajectoryBuilder(testing::ContainerEq(expected_sensor_ids), _, _)) + .WillOnce(testing::Return(3)); + int trajectory_id = stub_->AddTrajectoryBuilder( + expected_sensor_ids, trajectory_builder_options_, nullptr); + EXPECT_EQ(trajectory_id, 3); + EXPECT_CALL(*mock_map_builder_, GetTrajectoryBuilder(_)) + .WillRepeatedly(testing::Return(mock_trajectory_builder_.get())); + cartographer::mapping::TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + cartographer::sensor::ImuData imu_data{ + cartographer::common::FromUniversal(42), Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}; + EXPECT_CALL( + *mock_trajectory_builder_, + AddSensorData(testing::StrEq(kSensorId), + testing::Matcher(_))) + .WillOnce(testing::Return()); + trajectory_stub->AddSensorData(kSensorId, imu_data); + EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id)); + stub_->FinishTrajectory(trajectory_id); + server_->Shutdown(); +} + +TEST_F(ClientServerTest, LocalSlam2D) { + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = + stub_->AddTrajectoryBuilder({kRangeSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = + cartographer::mapping::test::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_stub->AddSensorData(kRangeSensorId, measurement); + } + WaitForLocalSlamResults(measurements.size()); + stub_->FinishTrajectory(trajectory_id); + EXPECT_EQ(local_slam_result_poses_.size(), measurements.size()); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); + server_->Shutdown(); +} + } // namespace } // namespace cartographer_grpc diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index c9e3255..b766262 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -207,4 +207,9 @@ void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) { } } +void MapBuilderServer::WaitUntilIdle() { + sensor_data_queue_.WaitUntilEmpty(); + map_builder_->pose_graph()->RunFinalOptimization(); +} + } // namespace cartographer_grpc diff --git a/cartographer_grpc/map_builder_server.h b/cartographer_grpc/map_builder_server.h index 04a49bf..9367198 100644 --- a/cartographer_grpc/map_builder_server.h +++ b/cartographer_grpc/map_builder_server.h @@ -90,6 +90,9 @@ class MapBuilderServer { // function to ever return. void WaitForShutdown(); + // Waits until all computation is finished (for testing). + void WaitUntilIdle(); + // Shuts down the gRPC server and the SLAM thread. void Shutdown();