Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/cloud/internal/client/pose_graph_stub.h"
00018
00019 #include "async_grpc/client.h"
00020 #include "cartographer/cloud/internal/handlers/delete_trajectory_handler.h"
00021 #include "cartographer/cloud/internal/handlers/get_all_submap_poses.h"
00022 #include "cartographer/cloud/internal/handlers/get_constraints_handler.h"
00023 #include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h"
00024 #include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h"
00025 #include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h"
00026 #include "cartographer/cloud/internal/handlers/get_trajectory_states_handler.h"
00027 #include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h"
00028 #include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h"
00029 #include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h"
00030 #include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h"
00031 #include "cartographer/cloud/internal/mapping/serialization.h"
00032 #include "cartographer/mapping/pose_graph.h"
00033 #include "cartographer/transform/transform.h"
00034 #include "glog/logging.h"
00035
00036 namespace cartographer {
00037 namespace cloud {
00038
00039 PoseGraphStub::PoseGraphStub(std::shared_ptr<::grpc::Channel> client_channel,
00040 const std::string& client_id)
00041 : client_channel_(client_channel), client_id_(client_id) {}
00042
00043 void PoseGraphStub::RunFinalOptimization() {
00044 google::protobuf::Empty request;
00045 async_grpc::Client<handlers::RunFinalOptimizationSignature> client(
00046 client_channel_);
00047 CHECK(client.Write(request));
00048 }
00049
00050 mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
00051 PoseGraphStub::GetAllSubmapData() const {
00052 LOG(FATAL) << "Not implemented";
00053 }
00054
00055 mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
00056 PoseGraphStub::GetAllSubmapPoses() const {
00057 google::protobuf::Empty request;
00058 async_grpc::Client<handlers::GetAllSubmapPosesSignature> client(
00059 client_channel_);
00060 CHECK(client.Write(request));
00061 mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose>
00062 submap_poses;
00063 for (const auto& submap_pose : client.response().submap_poses()) {
00064 submap_poses.Insert(
00065 mapping::SubmapId{submap_pose.submap_id().trajectory_id(),
00066 submap_pose.submap_id().submap_index()},
00067 mapping::PoseGraphInterface::SubmapPose{
00068 submap_pose.submap_version(),
00069 transform::ToRigid3(submap_pose.global_pose())});
00070 }
00071 return submap_poses;
00072 }
00073
00074 transform::Rigid3d PoseGraphStub::GetLocalToGlobalTransform(
00075 int trajectory_id) const {
00076 proto::GetLocalToGlobalTransformRequest request;
00077 request.set_trajectory_id(trajectory_id);
00078 async_grpc::Client<handlers::GetLocalToGlobalTransformSignature> client(
00079 client_channel_);
00080 CHECK(client.Write(request));
00081 return transform::ToRigid3(client.response().local_to_global());
00082 }
00083
00084 mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
00085 PoseGraphStub::GetTrajectoryNodes() const {
00086 LOG(FATAL) << "Not implemented";
00087 }
00088
00089 mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
00090 PoseGraphStub::GetTrajectoryNodePoses() const {
00091 google::protobuf::Empty request;
00092 async_grpc::Client<handlers::GetTrajectoryNodePosesSignature> client(
00093 client_channel_);
00094 CHECK(client.Write(request));
00095 mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
00096 for (const auto& node_pose : client.response().node_poses()) {
00097 absl::optional<mapping::TrajectoryNodePose::ConstantPoseData>
00098 constant_pose_data;
00099 if (node_pose.has_constant_pose_data()) {
00100 constant_pose_data = mapping::TrajectoryNodePose::ConstantPoseData{
00101 common::FromUniversal(node_pose.constant_pose_data().timestamp()),
00102 transform::ToRigid3(node_pose.constant_pose_data().local_pose())};
00103 }
00104 node_poses.Insert(
00105 mapping::NodeId{node_pose.node_id().trajectory_id(),
00106 node_pose.node_id().node_index()},
00107 mapping::TrajectoryNodePose{
00108 transform::ToRigid3(node_pose.global_pose()), constant_pose_data});
00109 }
00110 return node_poses;
00111 }
00112
00113 std::map<int, mapping::PoseGraphInterface::TrajectoryState>
00114 PoseGraphStub::GetTrajectoryStates() const {
00115 google::protobuf::Empty request;
00116 async_grpc::Client<handlers::GetTrajectoryStatesSignature> client(
00117 client_channel_);
00118 CHECK(client.Write(request));
00119 std::map<int, mapping::PoseGraphInterface::TrajectoryState>
00120 trajectories_state;
00121 for (const auto& entry : client.response().trajectories_state()) {
00122 trajectories_state[entry.first] = FromProto(entry.second);
00123 }
00124 return trajectories_state;
00125 }
00126
00127 std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses()
00128 const {
00129 google::protobuf::Empty request;
00130 async_grpc::Client<handlers::GetLandmarkPosesSignature> client(
00131 client_channel_);
00132 CHECK(client.Write(request));
00133 std::map<std::string, transform::Rigid3d> landmark_poses;
00134 for (const auto& landmark_pose : client.response().landmark_poses()) {
00135 landmark_poses[landmark_pose.landmark_id()] =
00136 transform::ToRigid3(landmark_pose.global_pose());
00137 }
00138 return landmark_poses;
00139 }
00140
00141 void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id,
00142 const transform::Rigid3d& global_pose,
00143 const bool frozen) {
00144 proto::SetLandmarkPoseRequest request;
00145 request.mutable_landmark_pose()->set_landmark_id(landmark_id);
00146 *request.mutable_landmark_pose()->mutable_global_pose() =
00147 transform::ToProto(global_pose);
00148 async_grpc::Client<handlers::SetLandmarkPoseSignature> client(
00149 client_channel_);
00150 CHECK(client.Write(request));
00151 }
00152
00153 void PoseGraphStub::DeleteTrajectory(int trajectory_id) {
00154 proto::DeleteTrajectoryRequest request;
00155 request.set_client_id(client_id_);
00156 request.set_trajectory_id(trajectory_id);
00157 async_grpc::Client<handlers::DeleteTrajectorySignature> client(
00158 client_channel_);
00159 ::grpc::Status status;
00160 client.Write(request, &status);
00161 if (!status.ok()) {
00162 LOG(ERROR) << "Failed to delete trajectory " << trajectory_id
00163 << " for client_id " << client_id_ << ": "
00164 << status.error_message();
00165 }
00166 }
00167
00168 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) const {
00169 proto::IsTrajectoryFinishedRequest request;
00170 request.set_trajectory_id(trajectory_id);
00171 async_grpc::Client<handlers::IsTrajectoryFinishedSignature> client(
00172 client_channel_);
00173 ::grpc::Status status;
00174 CHECK(client.Write(request, &status))
00175 << "Failed to check if trajectory " << trajectory_id
00176 << " is finished: " << status.error_message();
00177 return client.response().is_finished();
00178 }
00179
00180 bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) const {
00181 proto::IsTrajectoryFrozenRequest request;
00182 request.set_trajectory_id(trajectory_id);
00183 async_grpc::Client<handlers::IsTrajectoryFrozenSignature> client(
00184 client_channel_);
00185 ::grpc::Status status;
00186 CHECK(client.Write(request, &status))
00187 << "Failed to check if trajectory " << trajectory_id
00188 << " is frozen: " << status.error_message();
00189 return client.response().is_frozen();
00190 }
00191
00192 std::map<int, mapping::PoseGraphInterface::TrajectoryData>
00193 PoseGraphStub::GetTrajectoryData() const {
00194 LOG(FATAL) << "Not implemented";
00195 }
00196
00197 std::vector<mapping::PoseGraphInterface::Constraint>
00198 PoseGraphStub::constraints() const {
00199 google::protobuf::Empty request;
00200 async_grpc::Client<handlers::GetConstraintsSignature> client(client_channel_);
00201 ::grpc::Status status;
00202 CHECK(client.Write(request, &status))
00203 << "Failed to get constraints: " << status.error_message();
00204 return mapping::FromProto(client.response().constraints());
00205 }
00206
00207 mapping::proto::PoseGraph PoseGraphStub::ToProto(
00208 bool include_unfinished_submaps) const {
00209 LOG(FATAL) << "Not implemented";
00210 }
00211
00212 void PoseGraphStub::SetGlobalSlamOptimizationCallback(
00213 GlobalSlamOptimizationCallback callback) {
00214 LOG(FATAL) << "Not implemented";
00215 }
00216
00217 }
00218 }