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/handlers/add_trajectory_handler.h"
00018
00019 #include "absl/memory/memory.h"
00020 #include "async_grpc/rpc_handler.h"
00021 #include "cartographer/cloud/internal/map_builder_context_interface.h"
00022 #include "cartographer/cloud/internal/sensor/serialization.h"
00023 #include "cartographer/cloud/proto/map_builder_service.pb.h"
00024
00025 namespace cartographer {
00026 namespace cloud {
00027 namespace handlers {
00028
00029 void AddTrajectoryHandler::OnRequest(
00030 const proto::AddTrajectoryRequest& request) {
00031 auto local_slam_result_callback =
00032 GetUnsynchronizedContext<MapBuilderContextInterface>()
00033 ->GetLocalSlamResultCallbackForSubscriptions();
00034 std::set<mapping::TrajectoryBuilderInterface::SensorId> expected_sensor_ids;
00035 for (const auto& sensor_id : request.expected_sensor_ids()) {
00036 expected_sensor_ids.insert(FromProto(sensor_id));
00037 }
00038 const int trajectory_id =
00039 GetContext<MapBuilderContextInterface>()
00040 ->map_builder()
00041 .AddTrajectoryBuilder(expected_sensor_ids,
00042 request.trajectory_builder_options(),
00043 local_slam_result_callback);
00044 GetContext<MapBuilderContextInterface>()->RegisterClientIdForTrajectory(
00045 request.client_id(), trajectory_id);
00046 if (GetUnsynchronizedContext<MapBuilderContextInterface>()
00047 ->local_trajectory_uploader()) {
00048 auto trajectory_builder_options = request.trajectory_builder_options();
00049
00050
00051
00052
00053 trajectory_builder_options.clear_trajectory_builder_2d_options();
00054 trajectory_builder_options.clear_trajectory_builder_3d_options();
00055
00056
00057
00058 trajectory_builder_options.clear_pure_localization_trimmer();
00059
00060
00061 trajectory_builder_options.clear_initial_trajectory_pose();
00062
00063 auto status =
00064 GetContext<MapBuilderContextInterface>()
00065 ->local_trajectory_uploader()
00066 ->AddTrajectory(request.client_id(), trajectory_id,
00067 expected_sensor_ids, trajectory_builder_options);
00068 if (!status.ok()) {
00069 LOG(ERROR) << "Failed to create trajectory_id " << trajectory_id
00070 << " in uplink. Error: " << status.error_message();
00071 }
00072 }
00073
00074 auto response = absl::make_unique<proto::AddTrajectoryResponse>();
00075 response->set_trajectory_id(trajectory_id);
00076 Send(std::move(response));
00077 }
00078
00079 }
00080 }
00081 }