Autoware.Auto
add_covariance.hpp
Go to the documentation of this file.
1 // Copyright 2020 Apex.AI, Inc., Arm Limited
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //    http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 
19 
20 #ifndef COVARIANCE_INSERTION__ADD_COVARIANCE_HPP_
21 #define COVARIANCE_INSERTION__ADD_COVARIANCE_HPP_
22 
25 
26 #include <string>
27 #include <vector>
28 
29 namespace autoware
30 {
31 namespace covariance_insertion
32 {
33 
34 namespace detail
35 {
36 static constexpr auto kPoseTag = "pose";
37 static constexpr auto kTwistTag = "twist";
38 static constexpr auto kDirectlyTag = "directly";
39 } // namespace detail
40 
41 template<typename MsgT, typename ScalarT>
43  MsgT * msg,
44  const std::vector<ScalarT> & covariance,
45  const std::enable_if_t<has_covariance_member<MsgT>::value, std::string> & field)
46 {
47  if (!msg) {return;}
48  if (field != detail::kDirectlyTag) {
49  throw std::runtime_error("Message has covariance directly, but asked for field: " + field);
50  }
51  if (msg->covariance.size() != covariance.size()) {
52  throw std::runtime_error(
53  "Number of covariance entries does not match. The message has " +
54  std::to_string(msg->covariance.size()) + " entries, while there are " +
55  std::to_string(covariance.size()) + " entries in parameters of this node.");
56  }
57  for (auto i = 0U; i < covariance.size(); ++i) {
58  msg->covariance[i] = covariance[i];
59  }
60 }
61 
62 template<typename MsgT, typename ScalarT>
64  MsgT * msg,
65  const std::vector<ScalarT> & covariance,
66  const std::string & field)
67 {
68  if (!msg) {return;}
70  add_covariance(msg, covariance, detail::kDirectlyTag);
71  } else {
72  add_covariance(msg, covariance, field);
73  }
74 }
75 
76 template<typename MsgT, typename ScalarT>
78  MsgT * msg,
79  const std::vector<ScalarT> & covariance,
80  const std::enable_if_t<
83  !has_covariance_member<MsgT>::value, std::string> & field)
84 {
85  if (!msg) {return;}
86  if (field != detail::kTwistTag) {
87  throw std::runtime_error("Cannot set: " + field);
88  }
89  add_covariance_to_field(&msg->twist, covariance, field);
90 }
91 
92 template<typename MsgT, typename ScalarT>
94  MsgT * msg,
95  const std::vector<ScalarT> & covariance,
96  const std::enable_if_t<
99  !has_covariance_member<MsgT>::value, std::string> & field)
100 {
101  if (!msg) {return;}
102  if (field != detail::kPoseTag) {
103  throw std::runtime_error("Cannot set: " + field);
104  }
105  add_covariance_to_field(&msg->pose, covariance, field);
106 }
107 
108 template<typename MsgT, typename ScalarT>
110  MsgT * msg,
111  const std::vector<ScalarT> & covariance,
112  const std::enable_if_t<
115  !has_covariance_member<MsgT>::value, std::string> & field)
116 {
117  if (!msg) {return;}
118  if (field == detail::kTwistTag) {
119  add_covariance_to_field(&msg->twist, covariance, field);
120  } else if (field == detail::kPoseTag) {
121  add_covariance_to_field(&msg->pose, covariance, field);
122  } else {
123  throw std::runtime_error("Cannot set: " + field);
124  }
125 }
126 
127 } // namespace covariance_insertion
128 } // namespace autoware
129 
130 #endif // COVARIANCE_INSERTION__ADD_COVARIANCE_HPP_
autoware::covariance_insertion::has_twist_member
Definition: traits.hpp:47
autoware::covariance_insertion::detail::kTwistTag
static constexpr auto kTwistTag
Definition: add_covariance.hpp:37
autoware::covariance_insertion::add_covariance_to_field
void add_covariance_to_field(MsgT *msg, const std::vector< ScalarT > &covariance, const std::string &field)
Definition: add_covariance.hpp:63
autoware::covariance_insertion::detail::kPoseTag
static constexpr auto kPoseTag
Definition: add_covariance.hpp:36
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::covariance_insertion::has_pose_member
Definition: traits.hpp:39
autoware::covariance_insertion::detail::kDirectlyTag
static constexpr auto kDirectlyTag
Definition: add_covariance.hpp:38
autoware::covariance_insertion::has_covariance_member
Definition: traits.hpp:31
output_type_trait.hpp
traits.hpp
autoware::covariance_insertion::add_covariance
void add_covariance(MsgT *msg, const std::vector< ScalarT > &covariance, const std::enable_if_t< has_covariance_member< MsgT >::value, std::string > &field)
Definition: add_covariance.hpp:42