interval_calc.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00036 
00037 #include <sstream>
00038 #include <limits>
00039 #include <settlerlib/interval_calc.h>
00040 #include <ros/console.h>
00041 
00042 #define INTERVAL_DEBUG(fmt, ...) \
00043     ROS_DEBUG_NAMED("IntervalCalc", fmt,##__VA_ARGS__)
00044 
00045 
00046 using namespace std;
00047 using namespace settlerlib;
00048 
00049 calibration_msgs::Interval IntervalCalc::computeLatestInterval(const SortedDeque<DeflatedConstPtr>& signal,
00050                                                                const std::vector<double>& tolerances,
00051                                                                ros::Duration max_spacing)
00052 {
00053   if (max_spacing < ros::Duration(0,0))
00054   {
00055     ROS_WARN("max_spacing is negative (%.3f). Should be positive", max_spacing.toSec());
00056     max_spacing = -max_spacing;
00057   }
00058 
00059   if (signal.size() == 0)
00060   {
00061     ROS_WARN("Can't compute range of an empty signal");
00062     return calibration_msgs::Interval();
00063   }
00064 
00065   std::deque<DeflatedConstPtr>::const_reverse_iterator rev_it = signal.rbegin();
00066 
00067   assert(*rev_it);  // Make sure it's not a NULL pointer
00068 
00069   const unsigned int N = (*rev_it)->channels_.size();
00070 
00071   // if we have zero channels, the interval goes from zero to the end of time
00072   if ( N == 0 )
00073   {
00074     calibration_msgs::Interval result;
00075     result.end.sec = std::numeric_limits<uint32_t>::max();
00076     return result;
00077   }
00078 
00079   assert(tolerances.size() == N);
00080   vector<double> channel_max( (*rev_it)->channels_ );
00081   vector<double> channel_min( (*rev_it)->channels_ );
00082   vector<double> channel_range(N);
00083 
00084   calibration_msgs::Interval result;
00085   result.end   = (*signal.rbegin())->header.stamp;
00086   result.start = result.end;
00087 
00088 
00089   INTERVAL_DEBUG("Starting to walk along interval:");
00090   while( rev_it != signal.rend() )
00091   {
00092     if ( (*rev_it)->channels_.size() != N)
00093     {
00094       ROS_WARN("Num channels has changed. Cutting off interval prematurely ");
00095       return result;
00096     }
00097 
00098     ros::Duration cur_step = result.start - (*rev_it)->header.stamp;
00099     if ( cur_step > max_spacing)
00100     {
00101       INTERVAL_DEBUG("Difference between interval.start and it.stamp is [%.3fs]"
00102                      "Exceeds [%.3fs]", cur_step.toSec(), max_spacing.toSec());
00103       return result;
00104     }
00105 
00106     ostringstream max_debug;
00107     ostringstream min_debug;
00108     ostringstream range_debug;
00109     max_debug   << "  max:  ";
00110     min_debug   << "  min:  ";
00111     range_debug << "  range:";
00112     for (unsigned int i=0; i<N; i++)
00113     {
00114       channel_max[i]   = fmax( channel_max[i], (*rev_it)->channels_[i] );
00115       channel_min[i]   = fmin( channel_min[i], (*rev_it)->channels_[i] );
00116       channel_range[i] = channel_max[i] - channel_min[i];
00117 
00118       max_debug << "  " << channel_max[i];
00119       min_debug << "  " << channel_min[i];
00120       range_debug << "  " << channel_range[i];
00121     }
00122 
00123     INTERVAL_DEBUG("Current stats:\n%s\n%s\n%s",
00124                    max_debug.str().c_str(),
00125                    min_debug.str().c_str(),
00126                    range_debug.str().c_str());
00127 
00128     for (unsigned int i=0; i<N; i++)
00129     {
00130       if (channel_range[i] > tolerances[i])
00131       {
00132         INTERVAL_DEBUG("Channel %u range is %.3f.  Exceeds tolerance of %.3f", i, channel_range[i], tolerances[i]);
00133         return result;
00134       }
00135     }
00136 
00137     result.start = (*rev_it)->header.stamp;
00138 
00139     rev_it++;
00140   }
00141   return result;
00142 }


settlerlib
Author(s): Vijay Pradeep
autogenerated on Sat Jun 8 2019 19:41:50