cloud_accum.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2020, the mcl_3dl 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 #include <vector>
31 #include <string>
32 #include <functional>
33 
34 #include <ros/ros.h>
35 #include <sensor_msgs/PointCloud2.h>
36 
37 #include <mcl_3dl/cloud_accum.h>
38 
39 namespace mcl_3dl
40 {
42  const std::string& key,
43  const sensor_msgs::PointCloud2::ConstPtr& msg,
44  std::function<void()> process,
45  std::function<bool(const sensor_msgs::PointCloud2::ConstPtr&)> accumulate,
46  std::function<void()> clear)
47 {
48  clear();
49  if (accumulate(msg))
50  process();
51 }
52 
54  const std::string& key,
55  const sensor_msgs::PointCloud2::ConstPtr& msg,
56  std::function<void()> process,
57  std::function<bool(const sensor_msgs::PointCloud2::ConstPtr&)> accumulate,
58  std::function<void()> clear)
59 {
60  // If total count of the accumulated cloud exceeds limit,
61  // skip checking frame_id and force processing.
62  if (keys_.size() < accum_max_)
63  {
64  if (keys_.size() == 0 || keys_.front() != key)
65  {
66  if (accumulate(msg))
67  {
68  if (keys_.size() == 0)
69  cnt_accum_ = 1;
70 
71  keys_.push_back(key);
72  }
73  else
74  {
75  clear();
76  reset();
77  }
78  return;
79  }
80 
81  // Count number of clouds with the frame_id which was arrived
82  // at first in this accumulation.
83  if (cnt_accum_ < accum_)
84  {
85  if (accumulate(msg))
86  {
87  cnt_accum_++;
88  keys_.push_back(key);
89  }
90  else
91  {
92  clear();
93  reset();
94  }
95  return;
96  }
97 
98  // Received (accum_cloud_ + 1) of clouds now.
99  // Process already accumulated data and start next accumulation.
100  }
101  else
102  {
103  ROS_WARN(
104  "Number of the accumulated cloud exceeds limit. "
105  "Sensor with frame_id of %s may have been stopped.",
106  keys_.front().c_str());
107  }
108 
109  process();
110 
111  clear();
112  reset();
113 
114  if (accumulate(msg))
115  {
116  keys_.push_back(key);
117  cnt_accum_++;
118  }
119 }
120 
122 {
123  keys_.clear();
124  cnt_accum_ = 0;
125 }
126 } // namespace mcl_3dl
void push(const std::string &key, const sensor_msgs::PointCloud2::ConstPtr &msg, std::function< void()> process, std::function< bool(const sensor_msgs::PointCloud2::ConstPtr &)> accumulate, std::function< void()> clear) final
Definition: cloud_accum.cpp:53
#define ROS_WARN(...)
void push(const std::string &key, const sensor_msgs::PointCloud2::ConstPtr &msg, std::function< void()> process, std::function< bool(const sensor_msgs::PointCloud2::ConstPtr &)> accumulate, std::function< void()> clear) final
Definition: cloud_accum.cpp:41


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29