collated_trajectory_builder.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
20 #include "glog/logging.h"
21 
22 namespace cartographer {
23 namespace mapping {
24 
25 namespace {
26 
27 constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
28 
29 } // namespace
30 
32  sensor::Collator* const sensor_collator, const int trajectory_id,
33  const std::unordered_set<string>& expected_sensor_ids,
34  std::unique_ptr<GlobalTrajectoryBuilderInterface>
35  wrapped_trajectory_builder)
36  : sensor_collator_(sensor_collator),
37  trajectory_id_(trajectory_id),
38  wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
39  last_logging_time_(std::chrono::steady_clock::now()) {
41  trajectory_id, expected_sensor_ids,
42  [this](const string& sensor_id, std::unique_ptr<sensor::Data> data) {
43  HandleCollatedSensorData(sensor_id, std::move(data));
44  });
45 }
46 
48 
50  return wrapped_trajectory_builder_->num_submaps();
51 }
52 
54  const int submap_index) {
55  return wrapped_trajectory_builder_->GetSubmapData(submap_index);
56 }
57 
60  return wrapped_trajectory_builder_->pose_estimate();
61 }
62 
64  const string& sensor_id, std::unique_ptr<sensor::Data> data) {
65  sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data));
66 }
67 
69  const string& sensor_id, std::unique_ptr<sensor::Data> data) {
70  auto it = rate_timers_.find(sensor_id);
71  if (it == rate_timers_.end()) {
72  it = rate_timers_
73  .emplace(
74  std::piecewise_construct, std::forward_as_tuple(sensor_id),
75  std::forward_as_tuple(
76  common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
77  .first;
78  }
79  it->second.Pulse(data->time);
80 
81  if (std::chrono::steady_clock::now() - last_logging_time_ >
82  common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
83  for (const auto& pair : rate_timers_) {
84  LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
85  }
86  last_logging_time_ = std::chrono::steady_clock::now();
87  }
88 
89  switch (data->type) {
91  wrapped_trajectory_builder_->AddImuData(data->time,
92  data->imu.linear_acceleration,
93  data->imu.angular_velocity);
94  return;
95 
97  wrapped_trajectory_builder_->AddRangefinderData(
98  data->time, data->rangefinder.origin, data->rangefinder.ranges);
99  return;
100 
102  wrapped_trajectory_builder_->AddOdometerData(data->time,
103  data->odometer_pose);
104  return;
105  }
106  LOG(FATAL);
107 }
108 
109 } // namespace mapping
110 } // namespace cartographer
std::unique_ptr< GlobalTrajectoryBuilderInterface > wrapped_trajectory_builder_
std::map< string, common::RateTimer<> > rate_timers_
Duration FromSeconds(const double seconds)
Definition: time.cc:24
CollatedTrajectoryBuilder(sensor::Collator *sensor_collator, int trajectory_id, const std::unordered_set< string > &expected_sensor_ids, std::unique_ptr< GlobalTrajectoryBuilderInterface > wrapped_trajectory_builder)
void AddTrajectory(int trajectory_id, const std::unordered_set< string > &expected_sensor_ids, Callback callback)
Definition: collator.cc:22
void AddSensorData(const string &sensor_id, std::unique_ptr< sensor::Data > data) override
void AddSensorData(int trajectory_id, const string &sensor_id, std::unique_ptr< Data > data)
Definition: collator.cc:42
void HandleCollatedSensorData(const string &sensor_id, std::unique_ptr< sensor::Data > data)
std::chrono::steady_clock::time_point last_logging_time_


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58