Add 3D global SLAM grpc test. (#904)
parent
244cf615f5
commit
9793542957
|
@ -43,9 +43,11 @@ GenerateFakeRangeMeasurements(double travel_distance, double duration,
|
||||||
std::vector<cartographer::sensor::TimedPointCloudData> measurements;
|
std::vector<cartographer::sensor::TimedPointCloudData> measurements;
|
||||||
cartographer::sensor::TimedPointCloud point_cloud;
|
cartographer::sensor::TimedPointCloud point_cloud;
|
||||||
for (double angle = 0.; angle < M_PI; angle += 0.01) {
|
for (double angle = 0.; angle < M_PI; angle += 0.01) {
|
||||||
|
for (double height : {-0.4, -0.2, 0.0, 0.2, 0.4}) {
|
||||||
constexpr double kRadius = 5;
|
constexpr double kRadius = 5;
|
||||||
point_cloud.emplace_back(kRadius * std::cos(angle),
|
point_cloud.emplace_back(kRadius * std::cos(angle),
|
||||||
kRadius * std::sin(angle), 0., 0.);
|
kRadius * std::sin(angle), height, 0.);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized();
|
const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized();
|
||||||
const Eigen::Vector3f kVelocity = travel_distance / duration * kDirection;
|
const Eigen::Vector3f kVelocity = travel_distance / duration * kDirection;
|
||||||
|
|
|
@ -38,7 +38,7 @@ using testing::_;
|
||||||
namespace cartographer_grpc {
|
namespace cartographer_grpc {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
const SensorId kSensorId{SensorId::SensorType::IMU, "sensor"};
|
const SensorId kImuSensorId{SensorId::SensorType::IMU, "imu"};
|
||||||
const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"};
|
const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"};
|
||||||
constexpr double kDuration = 4.; // Seconds.
|
constexpr double kDuration = 4.; // Seconds.
|
||||||
constexpr double kTimeStep = 0.1; // Seconds.
|
constexpr double kTimeStep = 0.1; // Seconds.
|
||||||
|
@ -142,7 +142,7 @@ TEST_F(ClientServerTest, AddTrajectoryBuilder) {
|
||||||
server_->Start();
|
server_->Start();
|
||||||
InitializeStub();
|
InitializeStub();
|
||||||
int trajectory_id = stub_->AddTrajectoryBuilder(
|
int trajectory_id = stub_->AddTrajectoryBuilder(
|
||||||
{kSensorId}, trajectory_builder_options_, nullptr);
|
{kImuSensorId}, trajectory_builder_options_, nullptr);
|
||||||
stub_->FinishTrajectory(trajectory_id);
|
stub_->FinishTrajectory(trajectory_id);
|
||||||
server_->Shutdown();
|
server_->Shutdown();
|
||||||
}
|
}
|
||||||
|
@ -151,7 +151,7 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) {
|
||||||
InitializeServerWithMockMapBuilder();
|
InitializeServerWithMockMapBuilder();
|
||||||
server_->Start();
|
server_->Start();
|
||||||
InitializeStub();
|
InitializeStub();
|
||||||
std::set<SensorId> expected_sensor_ids = {kSensorId};
|
std::set<SensorId> expected_sensor_ids = {kImuSensorId};
|
||||||
EXPECT_CALL(
|
EXPECT_CALL(
|
||||||
*mock_map_builder_,
|
*mock_map_builder_,
|
||||||
AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _))
|
AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _))
|
||||||
|
@ -173,13 +173,13 @@ TEST_F(ClientServerTest, AddSensorData) {
|
||||||
server_->Start();
|
server_->Start();
|
||||||
InitializeStub();
|
InitializeStub();
|
||||||
int trajectory_id = stub_->AddTrajectoryBuilder(
|
int trajectory_id = stub_->AddTrajectoryBuilder(
|
||||||
{kSensorId}, trajectory_builder_options_, nullptr);
|
{kImuSensorId}, trajectory_builder_options_, nullptr);
|
||||||
TrajectoryBuilderInterface* trajectory_stub =
|
TrajectoryBuilderInterface* trajectory_stub =
|
||||||
stub_->GetTrajectoryBuilder(trajectory_id);
|
stub_->GetTrajectoryBuilder(trajectory_id);
|
||||||
cartographer::sensor::ImuData imu_data{
|
cartographer::sensor::ImuData imu_data{
|
||||||
cartographer::common::FromUniversal(42), Eigen::Vector3d(0., 0., 9.8),
|
cartographer::common::FromUniversal(42), Eigen::Vector3d(0., 0., 9.8),
|
||||||
Eigen::Vector3d::Zero()};
|
Eigen::Vector3d::Zero()};
|
||||||
trajectory_stub->AddSensorData(kSensorId.id, imu_data);
|
trajectory_stub->AddSensorData(kImuSensorId.id, imu_data);
|
||||||
stub_->FinishTrajectory(trajectory_id);
|
stub_->FinishTrajectory(trajectory_id);
|
||||||
server_->Shutdown();
|
server_->Shutdown();
|
||||||
}
|
}
|
||||||
|
@ -188,7 +188,7 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) {
|
||||||
InitializeServerWithMockMapBuilder();
|
InitializeServerWithMockMapBuilder();
|
||||||
server_->Start();
|
server_->Start();
|
||||||
InitializeStub();
|
InitializeStub();
|
||||||
std::set<SensorId> expected_sensor_ids = {kSensorId};
|
std::set<SensorId> expected_sensor_ids = {kImuSensorId};
|
||||||
EXPECT_CALL(
|
EXPECT_CALL(
|
||||||
*mock_map_builder_,
|
*mock_map_builder_,
|
||||||
AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _))
|
AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _))
|
||||||
|
@ -205,10 +205,10 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) {
|
||||||
Eigen::Vector3d::Zero()};
|
Eigen::Vector3d::Zero()};
|
||||||
EXPECT_CALL(*mock_trajectory_builder_,
|
EXPECT_CALL(*mock_trajectory_builder_,
|
||||||
AddSensorData(
|
AddSensorData(
|
||||||
::testing::StrEq(kSensorId.id),
|
::testing::StrEq(kImuSensorId.id),
|
||||||
::testing::Matcher<const cartographer::sensor::ImuData&>(_)))
|
::testing::Matcher<const cartographer::sensor::ImuData&>(_)))
|
||||||
.WillOnce(::testing::Return());
|
.WillOnce(::testing::Return());
|
||||||
trajectory_stub->AddSensorData(kSensorId.id, imu_data);
|
trajectory_stub->AddSensorData(kImuSensorId.id, imu_data);
|
||||||
EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id));
|
EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id));
|
||||||
stub_->FinishTrajectory(trajectory_id);
|
stub_->FinishTrajectory(trajectory_id);
|
||||||
server_->Shutdown();
|
server_->Shutdown();
|
||||||
|
@ -240,5 +240,49 @@ TEST_F(ClientServerTest, LocalSlam2D) {
|
||||||
server_->Shutdown();
|
server_->Shutdown();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST_F(ClientServerTest, GlobalSlam3D) {
|
||||||
|
map_builder_server_options_.mutable_map_builder_options()
|
||||||
|
->set_use_trajectory_builder_2d(false);
|
||||||
|
map_builder_server_options_.mutable_map_builder_options()
|
||||||
|
->set_use_trajectory_builder_3d(true);
|
||||||
|
map_builder_server_options_.mutable_map_builder_options()
|
||||||
|
->mutable_pose_graph_options()
|
||||||
|
->set_optimize_every_n_nodes(3);
|
||||||
|
trajectory_builder_options_.mutable_trajectory_builder_3d_options()
|
||||||
|
->mutable_motion_filter_options()
|
||||||
|
->set_max_distance_meters(0);
|
||||||
|
trajectory_builder_options_.mutable_trajectory_builder_3d_options()
|
||||||
|
->mutable_motion_filter_options()
|
||||||
|
->set_max_angle_radians(0);
|
||||||
|
InitializeRealServer();
|
||||||
|
server_->Start();
|
||||||
|
InitializeStub();
|
||||||
|
int trajectory_id = stub_->AddTrajectoryBuilder(
|
||||||
|
{kRangeSensorId, kImuSensorId}, 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) {
|
||||||
|
cartographer::sensor::ImuData imu_data{
|
||||||
|
measurement.time - cartographer::common::FromSeconds(kTimeStep / 2),
|
||||||
|
Eigen::Vector3d(0., 0., 9.8), Eigen::Vector3d::Zero()};
|
||||||
|
trajectory_stub->AddSensorData(kImuSensorId.id, imu_data);
|
||||||
|
trajectory_stub->AddSensorData(kRangeSensorId.id, measurement);
|
||||||
|
}
|
||||||
|
// First range data will not call back while initializing PoseExtrapolator, so
|
||||||
|
// expect one less.
|
||||||
|
WaitForLocalSlamResults(measurements.size() - 1);
|
||||||
|
stub_->FinishTrajectory(trajectory_id);
|
||||||
|
EXPECT_NEAR(kTravelDistance,
|
||||||
|
(local_slam_result_poses_.back().translation() -
|
||||||
|
local_slam_result_poses_.front().translation())
|
||||||
|
.norm(),
|
||||||
|
0.1 * kTravelDistance);
|
||||||
|
server_->Shutdown();
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
} // namespace cartographer_grpc
|
} // namespace cartographer_grpc
|
||||||
|
|
Loading…
Reference in New Issue