diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index 7af3a58..96e9888 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -108,6 +108,10 @@ bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) { LOG(FATAL) << "Not implemented"; } +bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) { + LOG(FATAL) << "Not implemented"; +} + std::map PoseGraphStub::GetTrajectoryData() { LOG(FATAL) << "Not implemented"; diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index f07002e..9c7e90d 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -40,6 +40,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { GetTrajectoryNodePoses() override; std::map GetLandmarkPoses() override; bool IsTrajectoryFinished(int trajectory_id) override; + bool IsTrajectoryFrozen(int trajectory_id) override; std::map GetTrajectoryData() override; std::vector constraints() override; diff --git a/cartographer/mapping/2d/pose_graph_2d.cc b/cartographer/mapping/2d/pose_graph_2d.cc index 65454b3..d8f1146 100644 --- a/cartographer/mapping/2d/pose_graph_2d.cc +++ b/cartographer/mapping/2d/pose_graph_2d.cc @@ -417,6 +417,10 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { }); } +bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) { + return frozen_trajectories_.count(trajectory_id) > 0; +} + void PoseGraph2D::AddSubmapFromProto( const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) { if (!submap.has_submap_2d()) { diff --git a/cartographer/mapping/2d/pose_graph_2d.h b/cartographer/mapping/2d/pose_graph_2d.h index a6230e5..b69222d 100644 --- a/cartographer/mapping/2d/pose_graph_2d.h +++ b/cartographer/mapping/2d/pose_graph_2d.h @@ -93,6 +93,7 @@ class PoseGraph2D : public PoseGraph { void FinishTrajectory(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override; + bool IsTrajectoryFrozen(int trajectory_id) override; void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, diff --git a/cartographer/mapping/3d/pose_graph_3d.cc b/cartographer/mapping/3d/pose_graph_3d.cc index d690ef1..55ecb3e 100644 --- a/cartographer/mapping/3d/pose_graph_3d.cc +++ b/cartographer/mapping/3d/pose_graph_3d.cc @@ -435,6 +435,10 @@ void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { }); } +bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) { + return frozen_trajectories_.count(trajectory_id) > 0; +} + void PoseGraph3D::AddSubmapFromProto( const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) { if (!submap.has_submap_3d()) { diff --git a/cartographer/mapping/3d/pose_graph_3d.h b/cartographer/mapping/3d/pose_graph_3d.h index c426160..9e0cc93 100644 --- a/cartographer/mapping/3d/pose_graph_3d.h +++ b/cartographer/mapping/3d/pose_graph_3d.h @@ -92,6 +92,7 @@ class PoseGraph3D : public PoseGraph { void FinishTrajectory(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override; + bool IsTrajectoryFrozen(int trajectory_id) override; void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) override; void AddNodeFromProto(const transform::Rigid3d& global_pose, diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 92c103f..fccd967 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -44,6 +44,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface { mapping::MapById()); MOCK_METHOD0(GetLandmarkPoses, std::map()); MOCK_METHOD1(IsTrajectoryFinished, bool(int)); + MOCK_METHOD1(IsTrajectoryFrozen, bool(int)); MOCK_METHOD0(GetTrajectoryData, std::map()); MOCK_METHOD0(constraints, std::vector()); diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index f3d4587..03fe1c9 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -111,6 +111,9 @@ class PoseGraphInterface { // Checks if the given trajectory is finished. virtual bool IsTrajectoryFinished(int trajectory_id) = 0; + // Checks if the given trajectory is frozen. + virtual bool IsTrajectoryFrozen(int trajectory_id) = 0; + // Returns the trajectory data. virtual std::map GetTrajectoryData() = 0;