NASA Astrobee Robot Software  0.16.7
Flight software for the Astrobee robot operating inside the International Space Station.
imu_augmentor_wrapper.h
Go to the documentation of this file.
1 /* Copyright (c) 2017, United States Government, as represented by the
2  * Administrator of the National Aeronautics and Space Administration.
3  *
4  * All rights reserved.
5  *
6  * The Astrobee platform is licensed under the Apache License, Version 2.0
7  * (the "License"); you may not use this file except in compliance with the
8  * License. You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
14  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
15  * License for the specific language governing permissions and limitations
16  * under the License.
17  */
18 #ifndef IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_
19 #define IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_
20 
21 #include <ff_msgs/EkfState.h>
22 #include <ff_msgs/FlightMode.h>
23 #include <ff_msgs/GraphState.h>
29 
30 #include <geometry_msgs/PoseWithCovarianceStamped.h>
31 #include <sensor_msgs/Imu.h>
32 
33 #include <memory>
34 #include <string>
35 #include <utility>
36 
37 namespace imu_augmentor {
39  public:
40  explicit ImuAugmentorWrapper(const std::string& graph_config_path_prefix = "");
41 
42  explicit ImuAugmentorWrapper(const ImuAugmentorParams& params);
43 
44  void LocalizationStateCallback(const ff_msgs::GraphState& loc_msg);
45 
46  void ImuCallback(const sensor_msgs::Imu& imu_msg);
47 
48  void FlightModeCallback(const ff_msgs::FlightMode& flight_mode);
49 
50  boost::optional<std::pair<localization_common::CombinedNavState, localization_common::CombinedNavStateCovariances>>
52 
53  boost::optional<ff_msgs::EkfState> LatestImuAugmentedLocalizationMsg();
54 
55  private:
56  void Initialize(const ImuAugmentorParams& params);
57 
59  localization_common::CombinedNavState& latest_imu_augmented_combined_nav_state,
60  localization_common::CombinedNavStateCovariances& latest_imu_augmented_covariances);
61 
62  bool LatestImuMeasurement(localization_measurements::ImuMeasurement& latest_imu_measurement);
63 
64  bool standstill() const;
65 
66  std::unique_ptr<ImuAugmentor> imu_augmentor_;
67  boost::optional<localization_common::CombinedNavState> latest_combined_nav_state_;
68  boost::optional<localization_common::CombinedNavState> latest_imu_augmented_combined_nav_state_;
69  boost::optional<localization_common::CombinedNavStateCovariances> latest_covariances_;
70  boost::optional<ff_msgs::GraphState> latest_loc_msg_;
71  std::unique_ptr<gtsam::TangentPreintegration> preintegration_helper_;
72  ImuAugmentorParams params_;
73  boost::optional<bool> standstill_;
74  localization_common::RateTimer loc_state_timer_ = localization_common::RateTimer("Loc State Msg");
75 };
76 } // namespace imu_augmentor
77 #endif // IMU_AUGMENTOR_IMU_AUGMENTOR_WRAPPER_H_
imu_augmentor::ImuAugmentorWrapper::ImuCallback
void ImuCallback(const sensor_msgs::Imu &imu_msg)
Definition: imu_augmentor_wrapper.cc:86
localization_measurements::ImuMeasurement
Definition: imu_measurement.h:30
imu_augmentor::ImuAugmentorWrapper::FlightModeCallback
void FlightModeCallback(const ff_msgs::FlightMode &flight_mode)
Definition: imu_augmentor_wrapper.cc:90
localization_common::CombinedNavState
Definition: combined_nav_state.h:48
imu_augmentor::ImuAugmentorWrapper::LatestImuAugmentedLocalizationMsg
boost::optional< ff_msgs::EkfState > LatestImuAugmentedLocalizationMsg()
Definition: imu_augmentor_wrapper.cc:126
imu_augmentor::ImuAugmentorWrapper
Definition: imu_augmentor_wrapper.h:38
localization_common::RateTimer
Definition: rate_timer.h:31
imu_augmentor::ImuAugmentorParams
Definition: imu_augmentor_params.h:24
rate_timer.h
imu_augmentor.h
combined_nav_state_covariances.h
combined_nav_state.h
imu_augmentor::ImuAugmentorWrapper::LocalizationStateCallback
void LocalizationStateCallback(const ff_msgs::GraphState &loc_msg)
Definition: imu_augmentor_wrapper.cc:63
imu_augmentor::ImuAugmentorWrapper::ImuAugmentorWrapper
ImuAugmentorWrapper(const std::string &graph_config_path_prefix="")
Definition: imu_augmentor_wrapper.cc:34
localization_common::CombinedNavStateCovariances
Definition: combined_nav_state_covariances.h:27
imu_measurement.h
imu_augmentor::ImuAugmentorWrapper::LatestImuAugmentedCombinedNavStateAndCovariances
boost::optional< std::pair< localization_common::CombinedNavState, localization_common::CombinedNavStateCovariances > > LatestImuAugmentedCombinedNavStateAndCovariances()
Definition: imu_augmentor_wrapper.cc:95
imu_augmentor
Definition: imu_augmentor.h:28