diff --git a/cartographer/cloud/internal/sensor/serialization.cc b/cartographer/cloud/internal/sensor/serialization.cc index 8127c4e..39a7acc 100644 --- a/cartographer/cloud/internal/sensor/serialization.cc +++ b/cartographer/cloud/internal/sensor/serialization.cc @@ -92,7 +92,7 @@ void CreateSensorDataForLocalSlamResult( for (const auto& insertion_submap : insertion_result.insertion_submaps) { // We only send the probability grid up if the submap is finished. auto* submap = proto->mutable_local_slam_result_data()->add_submaps(); - insertion_submap->ToProto(submap, insertion_submap->finished()); + *submap = insertion_submap->ToProto(insertion_submap->finished()); submap->mutable_submap_id()->set_trajectory_id(trajectory_id); submap->mutable_submap_id()->set_submap_index(starting_submap_index); ++starting_submap_index; diff --git a/cartographer/io/internal/mapping_state_serialization.cc b/cartographer/io/internal/mapping_state_serialization.cc index 4c42e9a..e6dd101 100644 --- a/cartographer/io/internal/mapping_state_serialization.cc +++ b/cartographer/io/internal/mapping_state_serialization.cc @@ -84,14 +84,13 @@ void SerializeSubmaps( for (const auto& submap_id_data : submap_data) { SerializedData proto; auto* const submap_proto = proto.mutable_submap(); + *submap_proto = submap_id_data.data.submap->ToProto( + /*include_probability_grid_data=*/submap_id_data.data.submap + ->finished()); submap_proto->mutable_submap_id()->set_trajectory_id( submap_id_data.id.trajectory_id); submap_proto->mutable_submap_id()->set_submap_index( submap_id_data.id.submap_index); - submap_id_data.data.submap->ToProto( - submap_proto, - /*include_probability_grid_data=*/submap_id_data.data.submap - ->finished()); writer->WriteProto(proto); } } diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index 5fd4cf9..dee0368 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -82,9 +82,10 @@ Submap2D::Submap2D(const proto::Submap2D& proto, set_finished(proto.finished()); } -void Submap2D::ToProto(proto::Submap* const proto, - bool include_probability_grid_data) const { - auto* const submap_2d = proto->mutable_submap_2d(); +proto::Submap Submap2D::ToProto( + const bool include_probability_grid_data) const { + proto::Submap proto; + auto* const submap_2d = proto.mutable_submap_2d(); *submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); submap_2d->set_num_range_data(num_range_data()); submap_2d->set_finished(finished()); @@ -92,6 +93,7 @@ void Submap2D::ToProto(proto::Submap* const proto, CHECK(grid_); *submap_2d->mutable_grid() = grid_->ToProto(); } + return proto; } void Submap2D::UpdateFromProto(const proto::Submap& proto) { diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index 2e97113..6c668d0 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -47,8 +47,7 @@ class Submap2D : public Submap { explicit Submap2D(const proto::Submap2D& proto, ValueConversionTables* conversion_tables); - void ToProto(proto::Submap* proto, - bool include_probability_grid_data) const override; + proto::Submap ToProto(bool include_probability_grid_data) const override; void UpdateFromProto(const proto::Submap& proto) override; void ToResponseProto(const transform::Rigid3d& global_submap_pose, diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 1508971..0f01763 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -103,8 +103,8 @@ TEST(Submap2DTest, ToFromProto) { common::make_unique(expected_map_limits, &conversion_tables), &conversion_tables); - proto::Submap proto; - expected.ToProto(&proto, true /* include_probability_grid_data */); + const proto::Submap proto = + expected.ToProto(true /* include_probability_grid_data */); EXPECT_TRUE(proto.has_submap_2d()); EXPECT_FALSE(proto.has_submap_3d()); const auto actual = Submap2D(proto.submap_2d(), &conversion_tables); diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 7f46b48..83b0a07 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -213,9 +213,10 @@ Submap3D::Submap3D(const proto::Submap3D& proto) set_finished(proto.finished()); } -void Submap3D::ToProto(proto::Submap* const proto, - bool include_probability_grid_data) const { - auto* const submap_3d = proto->mutable_submap_3d(); +proto::Submap Submap3D::ToProto( + const bool include_probability_grid_data) const { + proto::Submap proto; + auto* const submap_3d = proto.mutable_submap_3d(); *submap_3d->mutable_local_pose() = transform::ToProto(local_pose()); submap_3d->set_num_range_data(num_range_data()); submap_3d->set_finished(finished()); @@ -225,6 +226,7 @@ void Submap3D::ToProto(proto::Submap* const proto, *submap_3d->mutable_low_resolution_hybrid_grid() = low_resolution_hybrid_grid().ToProto(); } + return proto; } void Submap3D::UpdateFromProto(const proto::Submap& proto) { diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index 9b24020..a770936 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/cartographer/mapping/3d/submap_3d.h @@ -46,8 +46,7 @@ class Submap3D : public Submap { const transform::Rigid3d& local_submap_pose); explicit Submap3D(const proto::Submap3D& proto); - void ToProto(proto::Submap* proto, - bool include_probability_grid_data) const override; + proto::Submap ToProto(bool include_probability_grid_data) const override; void UpdateFromProto(const proto::Submap& proto) override; void ToResponseProto(const transform::Rigid3d& global_submap_pose, diff --git a/cartographer/mapping/3d/submap_3d_test.cc b/cartographer/mapping/3d/submap_3d_test.cc index b16fd41..3868d95 100644 --- a/cartographer/mapping/3d/submap_3d_test.cc +++ b/cartographer/mapping/3d/submap_3d_test.cc @@ -28,8 +28,8 @@ TEST(SubmapsTest, ToFromProto) { 0.05, 0.25, transform::Rigid3d(Eigen::Vector3d(1., 2., 0.), Eigen::Quaterniond(0., 0., 0., 1.))); - proto::Submap proto; - expected.ToProto(&proto, true /* include_probability_grid_data */); + const proto::Submap proto = + expected.ToProto(true /* include_probability_grid_data */); EXPECT_FALSE(proto.has_submap_2d()); EXPECT_TRUE(proto.has_submap_3d()); const auto actual = Submap3D(proto.submap_3d()); diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index cca356e..d464f78 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -62,8 +62,7 @@ class Submap { : local_pose_(local_submap_pose) {} virtual ~Submap() {} - virtual void ToProto(proto::Submap* proto, - bool include_probability_grid_data) const = 0; + virtual proto::Submap ToProto(bool include_probability_grid_data) const = 0; virtual void UpdateFromProto(const proto::Submap& proto) = 0; // Fills data into the 'response'.