136 lines
4.6 KiB
C++
136 lines
4.6 KiB
C++
/*
|
|
* Copyright 2018 The Cartographer Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include "cartographer_grpc/handlers/add_trajectory_handler.h"
|
|
#include "cartographer_grpc/sensor/serialization.h"
|
|
#include "cartographer_grpc/testing/handler_test.h"
|
|
#include "cartographer_grpc/testing/mock_map_builder.h"
|
|
#include "cartographer_grpc/testing/test_helpers.h"
|
|
#include "google/protobuf/text_format.h"
|
|
#include "gtest/gtest.h"
|
|
|
|
namespace cartographer_grpc {
|
|
namespace handlers {
|
|
namespace {
|
|
|
|
using ::testing::_;
|
|
using ::testing::ContainerEq;
|
|
using ::testing::Eq;
|
|
using ::testing::Return;
|
|
using ::testing::ReturnRef;
|
|
using ::testing::Test;
|
|
using ::testing::Truly;
|
|
|
|
const std::string kMessage = R"PROTO(
|
|
expected_sensor_ids {
|
|
id: "range_sensor"
|
|
type: RANGE
|
|
}
|
|
expected_sensor_ids {
|
|
id: "imu_sensor"
|
|
type: IMU
|
|
}
|
|
trajectory_builder_options {
|
|
trajectory_builder_2d_options {
|
|
min_range: 20
|
|
max_range: 30
|
|
}
|
|
pure_localization: true
|
|
initial_trajectory_pose {
|
|
relative_pose {
|
|
translation {
|
|
x: 1 y: 2 z: 3
|
|
}
|
|
rotation {
|
|
w: 4 x: 5 y: 6 z: 7
|
|
}
|
|
}
|
|
to_trajectory_id: 8
|
|
timestamp: 9
|
|
}
|
|
}
|
|
)PROTO";
|
|
|
|
class AddTrajectoryHandlerTest
|
|
: public testing::HandlerTest<AddTrajectoryHandler> {
|
|
public:
|
|
void SetUp() override {
|
|
testing::HandlerTest<AddTrajectoryHandler>::SetUp();
|
|
mock_map_builder_ =
|
|
cartographer::common::make_unique<testing::MockMapBuilder>();
|
|
EXPECT_CALL(*mock_map_builder_context_,
|
|
GetLocalSlamResultCallbackForSubscriptions())
|
|
.WillOnce(Return(nullptr));
|
|
EXPECT_CALL(*mock_map_builder_context_, map_builder())
|
|
.WillOnce(ReturnRef(*mock_map_builder_));
|
|
}
|
|
|
|
protected:
|
|
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
|
|
ParseSensorIds(const proto::AddTrajectoryRequest &request) {
|
|
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
|
|
expected_sensor_ids;
|
|
for (const auto &sensor_id : request.expected_sensor_ids()) {
|
|
expected_sensor_ids.insert(sensor::FromProto(sensor_id));
|
|
}
|
|
return expected_sensor_ids;
|
|
}
|
|
|
|
std::unique_ptr<testing::MockMapBuilder> mock_map_builder_;
|
|
};
|
|
|
|
TEST_F(AddTrajectoryHandlerTest, NoLocalSlamUploader) {
|
|
SetNoLocalTrajectoryUploader();
|
|
proto::AddTrajectoryRequest request;
|
|
EXPECT_TRUE(
|
|
google::protobuf::TextFormat::ParseFromString(kMessage, &request));
|
|
EXPECT_CALL(*mock_map_builder_,
|
|
AddTrajectoryBuilder(ContainerEq(ParseSensorIds(request)),
|
|
Truly(testing::BuildProtoPredicateEquals(
|
|
&request.trajectory_builder_options())),
|
|
_))
|
|
.WillOnce(Return(13));
|
|
test_server_->SendWrite(request);
|
|
EXPECT_EQ(test_server_->response().trajectory_id(), 13);
|
|
}
|
|
|
|
TEST_F(AddTrajectoryHandlerTest, WithLocalSlamUploader) {
|
|
SetMockLocalTrajectoryUploader();
|
|
proto::AddTrajectoryRequest request;
|
|
EXPECT_TRUE(
|
|
google::protobuf::TextFormat::ParseFromString(kMessage, &request));
|
|
EXPECT_CALL(*mock_map_builder_,
|
|
AddTrajectoryBuilder(ContainerEq(ParseSensorIds(request)),
|
|
Truly(testing::BuildProtoPredicateEquals(
|
|
&request.trajectory_builder_options())),
|
|
_))
|
|
.WillOnce(Return(13));
|
|
auto upstream_trajectory_builder_options =
|
|
request.trajectory_builder_options();
|
|
upstream_trajectory_builder_options.clear_trajectory_builder_2d_options();
|
|
upstream_trajectory_builder_options.clear_trajectory_builder_3d_options();
|
|
EXPECT_CALL(*mock_local_trajectory_uploader_,
|
|
AddTrajectory(Eq(13), ParseSensorIds(request),
|
|
Truly(testing::BuildProtoPredicateEquals(
|
|
&upstream_trajectory_builder_options))));
|
|
test_server_->SendWrite(request);
|
|
EXPECT_EQ(test_server_->response().trajectory_id(), 13);
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace handlers
|
|
} // namespace cartographer_grpc
|