Test TrajectoryBuilderStub (#780)

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<cartographer::sensor::TimedPointCloudData>
+GenerateFakeRangeMeasurements(double travel_distance, double duration,
+                              double time_step) {
+  std::vector<cartographer::sensor::TimedPointCloudData> 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

cartographer/internal/mapping/test_helpers.h

@@ -20,6 +20,7 @@
 #include <memory>
 
 #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<cartographer::sensor::TimedPointCloudData>
+GenerateFakeRangeMeasurements(double travel_distance, double duration,
+                              double time_step);
+
 }  // namespace test
 }  // namespace mapping
 }  // namespace cartographer

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<sensor::TimedPointCloudData> GenerateFakeRangeMeasurements() {
-  std::vector<sensor::TimedPointCloudData> 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);
   }

cartographer_grpc/client_server_test.cc

@@ -14,6 +14,9 @@
  * limitations under the License.
  */
 
+#include <condition_variable>
+#include <mutex>
+
 #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<const cartographer::mapping::NodeId>) {
+          std::unique_lock<std::mutex> 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<MapBuilderServer>(
         map_builder_server_options_, std::move(mock_map_builder));
     EXPECT_TRUE(server_ != nullptr);
+    mock_trajectory_builder_ =
+        cartographer::common::make_unique<MockTrajectoryBuilder>();
   }
 
   void InitializeStub() {
@@ -107,12 +146,24 @@ class ClientServerTest : public ::testing::Test {
     EXPECT_TRUE(stub_ != nullptr);
   }
 
+  void WaitForLocalSlamResults(size_t size) {
+    std::unique_lock<std::mutex> 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<MockTrajectoryBuilder> mock_trajectory_builder_;
   cartographer::mapping::proto::TrajectoryBuilderOptions
       trajectory_builder_options_;
   std::unique_ptr<MapBuilderServer> server_;
   std::unique_ptr<mapping::MapBuilderStub> stub_;
+  TrajectoryBuilderInterface::LocalSlamResultCallback
+      local_slam_result_callback_;
+  std::condition_variable local_slam_result_condition_;
+  std::mutex local_slam_result_mutex_;
+  std::vector<cartographer::transform::Rigid3d> 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<std::string> 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<const cartographer::sensor::ImuData&>(_)))
+      .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

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

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();