accumulator.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2017, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_ACCUMULATOR_H
31 #define COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_ACCUMULATOR_H
32 
33 #include <ros/ros.h>
34 
35 #include <list>
36 
37 template <typename T>
39 {
40 public:
41  class Points : public T
42  {
43  public:
45 
46  Points(const T& points, const ros::Time& stamp)
47  : T(points)
48  , stamp_(stamp)
49  {
50  }
51  };
52 
54  {
55  }
56 
57  explicit PointcloudAccumurator(const ros::Duration& duration)
58  {
59  reset(duration);
60  }
61 
62  void reset(const ros::Duration& duration)
63  {
64  time_to_hold_ = duration;
65  clear();
66  }
67 
68  void clear()
69  {
70  points_.clear();
71  }
72 
73  void push(const Points& points)
74  {
75  for (auto it = points_.begin(); it != points_.end(); ++it)
76  {
77  if (it->stamp_ + time_to_hold_ < points.stamp_)
78  {
79  it = points_.erase(it);
80  continue;
81  }
82  break;
83  }
84  points_.push_back(points);
85  }
86 
87  typename std::list<Points>::iterator begin()
88  {
89  return points_.begin();
90  }
91  typename std::list<Points>::iterator end()
92  {
93  return points_.end();
94  }
95 
96  typename std::list<Points>::const_iterator begin() const
97  {
98  return points_.cbegin();
99  }
100  typename std::list<Points>::const_iterator end() const
101  {
102  return points_.cend();
103  }
104 
105 protected:
107  std::list<Points> points_;
108 };
109 
110 #endif // COSTMAP_CSPACE_POINTCLOUD_ACCUMULATOR_ACCUMULATOR_H
std::list< Points >::iterator end()
Definition: accumulator.h:91
std::list< Points >::const_iterator end() const
Definition: accumulator.h:100
void push(const Points &points)
Definition: accumulator.h:73
std::list< Points >::const_iterator begin() const
Definition: accumulator.h:96
std::list< Points > points_
Definition: accumulator.h:107
ros::Duration time_to_hold_
Definition: accumulator.h:106
Points(const T &points, const ros::Time &stamp)
Definition: accumulator.h:46
std::list< Points >::iterator begin()
Definition: accumulator.h:87
PointcloudAccumurator(const ros::Duration &duration)
Definition: accumulator.h:57
void reset(const ros::Duration &duration)
Definition: accumulator.h:62


costmap_cspace
Author(s): Atsushi Watanabe
autogenerated on Tue Jul 9 2019 04:59:48