interval_calc.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
36 
37 #include <sstream>
38 #include <limits>
40 #include <ros/console.h>
41 
42 #define INTERVAL_DEBUG(fmt, ...) \
43  ROS_DEBUG_NAMED("IntervalCalc", fmt,##__VA_ARGS__)
44 
45 
46 using namespace std;
47 using namespace settlerlib;
48 
49 calibration_msgs::Interval IntervalCalc::computeLatestInterval(const SortedDeque<DeflatedConstPtr>& signal,
50  const std::vector<double>& tolerances,
51  ros::Duration max_spacing)
52 {
53  if (max_spacing < ros::Duration(0,0))
54  {
55  ROS_WARN("max_spacing is negative (%.3f). Should be positive", max_spacing.toSec());
56  max_spacing = -max_spacing;
57  }
58 
59  if (signal.size() == 0)
60  {
61  ROS_WARN("Can't compute range of an empty signal");
62  return calibration_msgs::Interval();
63  }
64 
65  std::deque<DeflatedConstPtr>::const_reverse_iterator rev_it = signal.rbegin();
66 
67  assert(*rev_it); // Make sure it's not a NULL pointer
68 
69  const unsigned int N = (*rev_it)->channels_.size();
70 
71  // if we have zero channels, the interval goes from zero to the end of time
72  if ( N == 0 )
73  {
74  calibration_msgs::Interval result;
75  result.end.sec = std::numeric_limits<uint32_t>::max();
76  return result;
77  }
78 
79  assert(tolerances.size() == N);
80  vector<double> channel_max( (*rev_it)->channels_ );
81  vector<double> channel_min( (*rev_it)->channels_ );
82  vector<double> channel_range(N);
83 
84  calibration_msgs::Interval result;
85  result.end = (*signal.rbegin())->header.stamp;
86  result.start = result.end;
87 
88 
89  INTERVAL_DEBUG("Starting to walk along interval:");
90  while( rev_it != signal.rend() )
91  {
92  if ( (*rev_it)->channels_.size() != N)
93  {
94  ROS_WARN("Num channels has changed. Cutting off interval prematurely ");
95  return result;
96  }
97 
98  ros::Duration cur_step = result.start - (*rev_it)->header.stamp;
99  if ( cur_step > max_spacing)
100  {
101  INTERVAL_DEBUG("Difference between interval.start and it.stamp is [%.3fs]"
102  "Exceeds [%.3fs]", cur_step.toSec(), max_spacing.toSec());
103  return result;
104  }
105 
106  ostringstream max_debug;
107  ostringstream min_debug;
108  ostringstream range_debug;
109  max_debug << " max: ";
110  min_debug << " min: ";
111  range_debug << " range:";
112  for (unsigned int i=0; i<N; i++)
113  {
114  channel_max[i] = fmax( channel_max[i], (*rev_it)->channels_[i] );
115  channel_min[i] = fmin( channel_min[i], (*rev_it)->channels_[i] );
116  channel_range[i] = channel_max[i] - channel_min[i];
117 
118  max_debug << " " << channel_max[i];
119  min_debug << " " << channel_min[i];
120  range_debug << " " << channel_range[i];
121  }
122 
123  INTERVAL_DEBUG("Current stats:\n%s\n%s\n%s",
124  max_debug.str().c_str(),
125  min_debug.str().c_str(),
126  range_debug.str().c_str());
127 
128  for (unsigned int i=0; i<N; i++)
129  {
130  if (channel_range[i] > tolerances[i])
131  {
132  INTERVAL_DEBUG("Channel %u range is %.3f. Exceeds tolerance of %.3f", i, channel_range[i], tolerances[i]);
133  return result;
134  }
135  }
136 
137  result.start = (*rev_it)->header.stamp;
138 
139  rev_it++;
140  }
141  return result;
142 }
#define INTERVAL_DEBUG(fmt,...)
std_msgs::Header * header(M &m)
Adds helper routines to the STL Deque for holding messages with headers.
Definition: sorted_deque.h:58
#define ROS_WARN(...)


settlerlib
Author(s): Vijay Pradeep
autogenerated on Thu Jun 6 2019 19:17:23