Apollo 10.0
自动驾驶开放平台
util.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
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
22#pragma once
23
24#include <algorithm>
25#include <iostream>
26#include <limits>
27#include <memory>
28#include <string>
29#include <utility>
30#include <vector>
31
32#include "cyber/common/log.h"
33#include "cyber/common/types.h"
36#include "modules/common_msgs/basic_msgs/geometry.pb.h"
37#include "modules/common_msgs/basic_msgs/pnc_point.pb.h"
38
43namespace apollo {
44namespace common {
45namespace util {
46template <typename ProtoA, typename ProtoB>
47bool IsProtoEqual(const ProtoA& a, const ProtoB& b) {
48 return a.GetTypeName() == b.GetTypeName() &&
49 a.SerializeAsString() == b.SerializeAsString();
50 // Test shows that the above method is 5 times faster than the
51 // API: google::protobuf::util::MessageDifferencer::Equals(a, b);
52}
53
54struct PairHash {
55 template <typename T, typename U>
56 size_t operator()(const std::pair<T, U>& pair) const {
57 return std::hash<T>()(pair.first) ^ std::hash<U>()(pair.second);
58 }
59};
60
61template <typename T>
62bool WithinBound(T start, T end, T value) {
63 return value >= start && value <= end;
64}
65
66PointENU operator+(const PointENU enu, const math::Vec2d& xy);
67
73template <typename T>
74void uniform_slice(const T start, const T end, uint32_t num,
75 std::vector<T>* sliced) {
76 if (!sliced || num == 0) {
77 return;
78 }
79 const T delta = (end - start) / num;
80 sliced->resize(num + 1);
81 T s = start;
82 for (uint32_t i = 0; i < num; ++i, s += delta) {
83 sliced->at(i) = s;
84 }
85 sliced->at(num) = end;
86}
87
96template <typename U, typename V>
97double DistanceXY(const U& u, const V& v) {
98 return std::hypot(u.x() - v.x(), u.y() - v.y());
99}
100
108template <typename U, typename V>
109bool SamePointXY(const U& u, const V& v) {
110 static constexpr double kMathEpsilonSqr = 1e-8 * 1e-8;
111 return (u.x() - v.x()) * (u.x() - v.x()) < kMathEpsilonSqr &&
112 (u.y() - v.y()) * (u.y() - v.y()) < kMathEpsilonSqr;
113}
114
116 const PathPoint& p2,
117 const double w1, const double w2);
118
119// Test whether two float or double numbers are equal.
120// ulp: units in the last place.
121template <typename T>
122typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
123IsFloatEqual(T x, T y, int ulp = 2) {
124 // the machine epsilon has to be scaled to the magnitude of the values used
125 // and multiplied by the desired precision in ULPs (units in the last place)
126 return std::fabs(x - y) <
127 std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp
128 // unless the result is subnormal
129 || std::fabs(x - y) < std::numeric_limits<T>::min();
130}
131} // namespace util
132} // namespace common
133} // namespace apollo
134
135template <typename T>
137 public:
138 typedef int (T::*Function)();
140 std::string fun_name_;
141};
142
143template <typename T, size_t count>
144bool ExcuteAllFunctions(T* obj, FunctionInfo<T> fun_list[]) {
145 for (size_t i = 0; i < count; i++) {
146 if ((obj->*(fun_list[i].function_))() != apollo::cyber::SUCC) {
147 AERROR << fun_list[i].fun_name_ << " failed.";
148 return false;
149 }
150 }
151 return true;
152}
153
154#define EXEC_ALL_FUNS(type, obj, list) \
155 ExcuteAllFunctions<type, sizeof(list) / sizeof(FunctionInfo<type>)>(obj, list)
156
157template <typename A, typename B>
158std::ostream& operator<<(std::ostream& os, std::pair<A, B>& p) {
159 return os << "first: " << p.first << ", second: " << p.second;
160}
161
162#define UNIQUE_LOCK_MULTITHREAD(mutex_type) \
163 std::unique_ptr<std::unique_lock<std::mutex>> lock_ptr = nullptr; \
164 if (FLAGS_multithread_run) { \
165 lock_ptr.reset(new std::unique_lock<std::mutex>(mutex_type)); \
166 }
int(T::* Function)()
Definition util.h:138
std::string fun_name_
Definition util.h:140
Function function_
Definition util.h:139
Implements a class of 2-dimensional vectors.
Definition vec2d.h:42
#define AERROR
Definition log.h:44
std::ostream & operator<<(std::ostream &os, std::pair< A, B > &p)
Definition util.h:158
bool ExcuteAllFunctions(T *obj, FunctionInfo< T > fun_list[])
Definition util.h:144
bool IsProtoEqual(const ProtoA &a, const ProtoB &b)
Definition util.h:47
bool WithinBound(T start, T end, T value)
Definition util.h:62
bool SamePointXY(const U &u, const V &v)
Check if two points u and v are the same point on XY dimension.
Definition util.h:109
double DistanceXY(const U &u, const V &v)
calculate the distance beteween Point u and Point v, which are all have member function x() and y() i...
Definition util.h:97
std::enable_if<!std::numeric_limits< T >::is_integer, bool >::type IsFloatEqual(T x, T y, int ulp=2)
Definition util.h:123
void uniform_slice(const T start, const T end, uint32_t num, std::vector< T > *sliced)
uniformly slice a segment [start, end] to num + 1 pieces the result sliced will contain the n + 1 poi...
Definition util.h:74
PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint &p1, const PathPoint &p2, const double w1, const double w2)
Definition util.cc:34
class register implement
Definition arena_queue.h:37
size_t operator()(const std::pair< T, U > &pair) const
Definition util.h:56
Defines the Vec2d class.