Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22859] pkg/trunk
|
From: <vij...@us...> - 2009-08-25 17:42:16
|
Revision: 22859
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22859&view=rev
Author: vijaypradeep
Date: 2009-08-25 17:41:58 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
Moving calibration stack into stacks
Added Paths:
-----------
pkg/trunk/stacks/calibration/
pkg/trunk/stacks/calibration/calibration_msgs/
pkg/trunk/stacks/calibration/calibration_msgs/CMakeLists.txt
pkg/trunk/stacks/calibration/calibration_msgs/Makefile
pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml
pkg/trunk/stacks/calibration/calibration_msgs/msg/
pkg/trunk/stacks/calibration/calibration_msgs/msg/Interval.msg
pkg/trunk/stacks/calibration/joint_states_settler/
pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt
pkg/trunk/stacks/calibration/joint_states_settler/Makefile
pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
pkg/trunk/stacks/calibration/settlerlib/
pkg/trunk/stacks/calibration/settlerlib/CMakeLists.txt
pkg/trunk/stacks/calibration/settlerlib/Makefile
pkg/trunk/stacks/calibration/settlerlib/include/
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/sorted_deque.h
pkg/trunk/stacks/calibration/settlerlib/manifest.xml
pkg/trunk/stacks/calibration/settlerlib/test/
pkg/trunk/stacks/calibration/settlerlib/test/CMakeLists.txt
pkg/trunk/stacks/calibration/settlerlib/test/sorted_deque_unittest.cpp
pkg/trunk/stacks/calibration/stack.xml
Removed Paths:
-------------
pkg/trunk/calibration/calibration_msgs/CMakeLists.txt
pkg/trunk/calibration/calibration_msgs/Makefile
pkg/trunk/calibration/calibration_msgs/manifest.xml
pkg/trunk/calibration/calibration_msgs/msg/Interval.msg
pkg/trunk/calibration/joint_states_settler/CMakeLists.txt
pkg/trunk/calibration/joint_states_settler/Makefile
pkg/trunk/calibration/joint_states_settler/manifest.xml
pkg/trunk/calibration/settlerlib/CMakeLists.txt
pkg/trunk/calibration/settlerlib/Makefile
pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h
pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h
pkg/trunk/calibration/settlerlib/manifest.xml
pkg/trunk/calibration/settlerlib/test/CMakeLists.txt
pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp
pkg/trunk/calibration/stack.xml
Deleted: pkg/trunk/calibration/calibration_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/calibration_msgs/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,22 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
Deleted: pkg/trunk/calibration/calibration_msgs/Makefile
===================================================================
--- pkg/trunk/calibration/calibration_msgs/Makefile 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration/calibration_msgs/manifest.xml
===================================================================
--- pkg/trunk/calibration/calibration_msgs/manifest.xml 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,14 +0,0 @@
-<package>
- <description brief="calibration_msgs">
-
- calibration_msgs
-
- </description>
- <author>Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/calibration_msgs</url>
-
-</package>
-
-
Deleted: pkg/trunk/calibration/calibration_msgs/msg/Interval.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/Interval.msg 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/msg/Interval.msg 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,2 +0,0 @@
-time start
-time end
Deleted: pkg/trunk/calibration/joint_states_settler/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,30 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
Deleted: pkg/trunk/calibration/joint_states_settler/Makefile
===================================================================
--- pkg/trunk/calibration/joint_states_settler/Makefile 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/joint_states_settler/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration/joint_states_settler/manifest.xml
===================================================================
--- pkg/trunk/calibration/joint_states_settler/manifest.xml 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/joint_states_settler/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,17 +0,0 @@
-<package>
- <description brief="joint_states_settler">
-
- joint_states_settler
-
- </description>
- <author>Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/joint_states_settler</url>
- <depend package="roscpp"/>
- <depend package="actionlib"/>
- <depend package="mechanism_msgs"/>
-
-</package>
-
-
Deleted: pkg/trunk/calibration/settlerlib/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/settlerlib/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,31 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
-add_subdirectory(test EXCLUDE_FROM_ALL)
Deleted: pkg/trunk/calibration/settlerlib/Makefile
===================================================================
--- pkg/trunk/calibration/settlerlib/Makefile 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h
===================================================================
--- pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,56 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef SETTLERLIB_DEFLATED_H_
-#define SETTLERLIB_DEFLATED_H_
-
-#include <boost/shared_ptr.hpp>
-#include "roslib/Header.h"
-
-namespace settlerlib
-{
-
-class Deflated
-{
-public:
- roslib::Header header; // Need header to put in cache
- std::vector<double> channel_;
-};
-
-}
-
-#endif
-
-
-
Deleted: pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h
===================================================================
--- pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,337 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2009, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include <deque>
-#include <boost/shared_ptr.hpp>
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
-#include "ros/time.h"
-#include "ros/console.h"
-
-#ifndef SETTLERLIB_SORTED_DEQUE_H_
-#define SETTLERLIB_SORTED_DEQUE_H_
-
-#define DEQUE_DEBUG(fmt, ...) \
- ROS_DEBUG_NAMED(logger_.c_str(), fmt,##__VA_ARGS__)
-
-namespace settlerlib
-{
-
-/**
- * \brief Adds helper routines to the STL Deque for holding messages with headers
- *
- * Provides functionality to store a deque of messages that are sorted by timestamp. Users can
- * then extract specific intervals of messages. Old messages fall off the back of the deque
- */
-template <class M>
-class SortedDeque : public std::deque<M>
-{
-public:
-
- using std::deque<M>::size;
- using std::deque<M>::front;
- using std::deque<M>::pop_front;
- using std::deque<M>::begin;
- using std::deque<M>::end;
- using std::deque<M>::rbegin;
- using std::deque<M>::rend;
- using std::deque<M>::insert;
- using std::deque<M>::at;
- using std::deque<M>::erase;
-
- /**
- * \brief Assumes that '.header.stamp' can be used to get the stamp used for sorting
- * \param logger name of rosconsole logger to display debugging output from this deque
- */
- SortedDeque(std::string logger = "deque") : std::deque<M>(), logger_(logger)
- {
- getStamp = &SortedDeque<M>::getStructStamp;
- }
-
- /**
- * \brief Advanced constructor, allowing user to specific the timestamp getter method
- * \param getStampFunc Function pointer specific how to get timestamp from the contained datatype
- * \param logger name of rosconsole logger to display debugging output from this deque
- */
- SortedDeque(boost::function<const ros::Time&(const M&)> getStampFunc,
- std::string logger = "deque") : std::deque<M>(), logger_(logger)
- {
- getStamp = getStampFunc;
- }
-
- /**
- * \brief Set the maximum # of elements this deque can hold. Older elems are popped once the length is exeeded
- * \param max_size The maximum # of elems to hold. Passing in 0 implies that the queue can grow indefinitely (which is a recipe for a memory leak)
- */
- void setMaxSize(unsigned int max_size)
- {
- max_size_ = max_size ;
- }
-
- /**
- * \brief Add a new element to the deque, correctly sorted by timestamp
- * \param msg the element to add
- */
- void add(const M& msg)
- {
- DEQUE_DEBUG("Called add()");
- DEQUE_DEBUG_STATS(" ");
- if (max_size_ != 0)
- {
- while (size() >= max_size_) // Keep popping off old data until we have space for a new msg
- {
- pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
- DEQUE_DEBUG(" Popping element");
- DEQUE_DEBUG_STATS(" ");
- }
- }
-
- typename std::deque<M>::reverse_iterator rev_it = rbegin();
-
- // Keep walking backwards along deque until we hit the beginning,
- // or until we find a timestamp that's smaller than (or equal to) msg's timestamp
- while(rev_it != rend() && getStamp(*rev_it) > getStamp(msg))
- rev_it++;
-
- // Add msg to the cache
- insert(rev_it.base(), msg);
- DEQUE_DEBUG(" Done inserting");
- DEQUE_DEBUG_STATS(" ");
- }
-
- /**
- * \brief Extract all the elements that occur in the interval between the start and end times
- * \param start The start of the interval
- * \param end The end of the interval
- */
- std::vector<M> getInterval(const ros::Time& start, const ros::Time& end)
- {
- // Find the starting index. (Find the first index after [or at] the start of the interval)
- unsigned int start_index = 0 ;
- while(start_index < size() &&
- getStamp(at(start_index)) < start)
- {
- start_index++ ;
- }
-
- // Find the ending index. (Find the first index after the end of interval)
- unsigned int end_index = start_index ;
- while(end_index < size() &&
- getStamp(at(end_index)) <= end)
- {
- end_index++ ;
- }
-
- std::vector<M> interval_elems ;
- interval_elems.reserve(end_index - start_index) ;
- for (unsigned int i=start_index; i<end_index; i++)
- {
- interval_elems.push_back(at(i)) ;
- }
-
- return interval_elems ;
- }
-
- /**
- * Retrieve the smallest interval of messages that surrounds an interval from start to end.
- * If the messages in the cache do not surround (start,end), then this will return the interval
- * that gets closest to surrounding (start,end)
- */
- std::vector<M> getSurroundingInterval(const ros::Time& start, const ros::Time& end)
- {
- // Find the starting index. (Find the first index after [or at] the start of the interval)
- unsigned int start_index = size()-1;
- while(start_index > 0 &&
- getStamp(at(start_index)) > start)
- {
- start_index--;
- }
- unsigned int end_index = start_index;
- while(end_index < size()-1 &&
- getStamp(at(end_index)) < end)
- {
- end_index++;
- }
-
- std::vector<M> interval_elems;
- interval_elems.reserve(end_index - start_index + 1) ;
- for (unsigned int i=start_index; i<=end_index; i++)
- {
- interval_elems.push_back(at(i)) ;
- }
-
- return interval_elems;
- }
-
- /**
- * \brief Grab the oldest element that occurs right before the specified time.
- * \param time The time that must occur after the elem
- * \param out Output: Stores the extracted elem
- * \return False there are no elems before the specified time
- **/
- bool getElemBeforeTime(const ros::Time& time, M& out)
- {
- unsigned int i=0 ;
- int elem_index = -1 ;
- while (i<size() &&
- getStamp(at(i)) < time)
- {
- elem_index = i ;
- i++ ;
- }
-
- if (elem_index >= 0)
- {
- out = at(elem_index);
- return true;
- }
- //out = M();
- return false;
- }
-
- /**
- * \brief Grab the oldest element that occurs right after the specified time.
- * \param time The time that must occur before the elem
- * \param out Output: Stores the extracted elem
- * \return False there are no elems after the specified time
- */
- bool getElemAfterTime(const ros::Time& time, M& out)
- {
- int i=size()-1 ;
- int elem_index = -1 ;
- while (i>=0 &&
- getStamp(at(i)) > time)
- {
- elem_index = i ;
- i-- ;
- }
-
- if (elem_index >= 0)
- {
- out = at(elem_index);
- return true;
- }
- //out = M();
- return false;
- }
-
- /**
- * \brief Get the elem that occurs closest to the specified time
- * \param time The time that we want to closest elem to
- * \param out Output: Stores the extracted elem
- * \return False if the queue is empty
- */
- bool getClosestElem(const ros::Time& time, M& out)
- {
- if (size() == 0)
- return false;
-
- typename std::deque<M>::iterator it = begin();
- typename std::deque<M>::iterator best = it;
-
- double best_diff = fabs( (time - getStamp(*best)).toSec());
-
- while (it != end())
- {
- double cur_diff = fabs( (time - getStamp(*it)).toSec());
- if (cur_diff < best_diff)
- {
- best_diff = cur_diff;
- best = it;
- }
- ++it;
- }
- out = *best;
- return true;
- }
-
-
- /**
- * \brief Removes all elements that occur before the specified time
- * \param time All elems that occur before this are removed from the deque
- */
- void removeAllBeforeTime(const ros::Time& time)
- {
- DEQUE_DEBUG("Called removeAllBeforeTime()");
- DEQUE_DEBUG(" Erasing all elems before time: %u %u", time.sec, time.nsec);
- typename std::deque<M>::iterator it = begin();
-
- while (size() > 0 && getStamp(front()) < time)
- {
- DEQUE_DEBUG(" Erasing elem at time: %u, %u", getStamp(front()).sec, getStamp(front()).nsec);
- pop_front();
- DEQUE_DEBUG(" Erased an elem");
- DEQUE_DEBUG_STATS(" ");
- }
- DEQUE_DEBUG(" Done erasing elems");
- }
-
- static const ros::Time& getPtrStamp(const M& m)
- {
- return m->header.stamp;
- }
-
- static const ros::Time& getStructStamp(const M& m)
- {
- return m.header.stamp;
- }
-
- static const ros::Time& getHeaderStamp(const M& m)
- {
- return m.stamp;
- }
-private:
- unsigned int max_size_;
- std::string logger_;
-
-
- /*const ros::Time& getStamp(const M& m)
- {
- return getStructStamp(m);
- }*/
-
- boost::function<const ros::Time&(const M&)> getStamp;
-
- inline void DEQUE_DEBUG_STATS(const std::string& prefix)
- {
- DEQUE_DEBUG("%sdeque.size(): %u max_size: %u", prefix.c_str(), (unsigned int) size(), max_size_);
- }
-};
-
-}
-
-#undef DEQUE_DEBUG
-#undef DEQUE_WARN
-
-#endif
Deleted: pkg/trunk/calibration/settlerlib/manifest.xml
===================================================================
--- pkg/trunk/calibration/settlerlib/manifest.xml 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,14 +0,0 @@
-<package>
- <description brief="A set of routines to help with creating a settler">
- Defines helper functions and routines that greatly help when trying to create a settler
- for a specific sensor channel.
- </description>
- <author>Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/settlerlib</url>
- <depend package="roscpp"/>
-
-</package>
-
-
Deleted: pkg/trunk/calibration/settlerlib/test/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/settlerlib/test/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/test/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,5 +0,0 @@
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
-
-# ********** Tests **********
-
-rospack_add_gtest(test/sorted_deque_unittest sorted_deque_unittest.cpp)
Deleted: pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp
===================================================================
--- pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,222 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-
-#include <gtest/gtest.h>
-
-#include "settlerlib/sorted_deque.h"
-
-using namespace std;
-using namespace settlerlib;
-
-struct Header
-{
- ros::Time stamp ;
-} ;
-
-
-struct Msg
-{
- Header header ;
- int data ;
-} ;
-
-void fillEasy(SortedDeque<Msg>& sd, unsigned int start, unsigned int end)
-{
- for (unsigned int i=start; i < end; i++)
- {
- Msg msg ;
- msg.data = i ;
- msg.header.stamp.fromSec(i*10) ;
-
- sd.add(msg) ;
- }
-}
-
-TEST(SortedDeque, easyInterval)
-{
- SortedDeque<Msg> sd;
- sd.setMaxSize(10);
-
- fillEasy(sd, 0, 5);
-
- vector<Msg> interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
-
- EXPECT_EQ(interval_data.size(), (unsigned int) 3) ;
- EXPECT_EQ(interval_data[0].data, 1) ;
- EXPECT_EQ(interval_data[1].data, 2) ;
- EXPECT_EQ(interval_data[2].data, 3) ;
-
- // Look for an interval past the end of the cache
- interval_data = sd.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
-
- // Look for an interval that fell off the back of the cache
- fillEasy(sd, 5, 20) ;
- interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
-}
-
-TEST(SortedDeque, easySurroundingInterval)
-{
- SortedDeque<Msg> cache;
- cache.setMaxSize(10);
- fillEasy(cache, 1, 6);
-
- vector<Msg> interval_data;
- interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 4);
- EXPECT_EQ(interval_data[0].data, 1);
- EXPECT_EQ(interval_data[1].data, 2);
- EXPECT_EQ(interval_data[2].data, 3);
- EXPECT_EQ(interval_data[3].data, 4);
-
- interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 4);
- EXPECT_EQ(interval_data[0].data, 1);
-
- interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 2);
- EXPECT_EQ(interval_data[0].data, 3);
- EXPECT_EQ(interval_data[1].data, 4);
-
- interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 1);
- EXPECT_EQ(interval_data[0].data, 5);
-}
-
-Msg buildMsg(double time, int data)
-{
- Msg msg;
- msg.data = data;
- msg.header.stamp.fromSec(time);
- return msg;
-}
-
-TEST(SortedDeque, easyUnsorted)
-{
- SortedDeque<Msg> cache;
- cache.setMaxSize(10);
-
- cache.add(buildMsg(10.0, 1)) ;
- cache.add(buildMsg(30.0, 3)) ;
- cache.add(buildMsg(70.0, 7)) ;
- cache.add(buildMsg( 5.0, 0)) ;
- cache.add(buildMsg(20.0, 2)) ;
-
- vector<Msg> interval_data = cache.getInterval(ros::Time().fromSec(3), ros::Time().fromSec(15)) ;
-
- EXPECT_EQ(interval_data.size(), (unsigned int) 2) ;
- EXPECT_EQ(interval_data[0].data, 0) ;
- EXPECT_EQ(interval_data[1].data, 1) ;
-
- // Grab all the data
- interval_data = cache.getInterval(ros::Time().fromSec(0), ros::Time().fromSec(80)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 5) ;
- EXPECT_EQ(interval_data[0].data, 0) ;
- EXPECT_EQ(interval_data[1].data, 1) ;
- EXPECT_EQ(interval_data[2].data, 2) ;
- EXPECT_EQ(interval_data[3].data, 3) ;
- EXPECT_EQ(interval_data[4].data, 7) ;
-}
-
-TEST(SortedDeque, easyElemBeforeAfter)
-{
- SortedDeque<Msg> cache;
- cache.setMaxSize(10);
- Msg elem;
-
- fillEasy(cache, 5, 10) ;
-
- bool result;
- result = cache.getElemAfterTime( ros::Time().fromSec(85.0), elem);
-
- ASSERT_TRUE(result);
- EXPECT_EQ(elem.data, 9);
-
- result = cache.getElemBeforeTime( ros::Time().fromSec(85.0), elem);
- ASSERT_TRUE(result) ;
- EXPECT_EQ(elem.data, 8) ;
-
- result = cache.getElemBeforeTime( ros::Time().fromSec(45.0), elem);
- EXPECT_FALSE(result) ;
-}
-
-TEST(SortedDeque, easyRemoval)
-{
- SortedDeque<Msg> sd;
- sd.setMaxSize(20);
-
- fillEasy(sd, 1, 10);
-
- vector<Msg> interval_data;
- interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(105)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 9) ;
-
- sd.removeAllBeforeTime(ros::Time().fromSec(35));
- interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(105)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 6);
- EXPECT_EQ(interval_data[0].data, 4) ;
- EXPECT_EQ(interval_data[1].data, 5) ;
- EXPECT_EQ(interval_data[2].data, 6) ;
- EXPECT_EQ(interval_data[3].data, 7) ;
- EXPECT_EQ(interval_data[4].data, 8) ;
- EXPECT_EQ(interval_data[5].data, 9) ;
-}
-
-TEST(SortedDeque, easyClosest)
-{
- SortedDeque<Msg> sd;
- sd.setMaxSize(20);
-
- fillEasy(sd, 1, 10);
-
- bool found;
- Msg elem;
-
- found = sd.getClosestElem(ros::Time().fromSec(5), elem) ;
- EXPECT_TRUE(found) ;
- EXPECT_EQ(elem.data, 1) ;
-
- found = sd.getClosestElem(ros::Time().fromSec(20), elem) ;
- EXPECT_TRUE(found) ;
- EXPECT_EQ(elem.data, 2) ;
-
-}
-
-
-int main(int argc, char **argv){
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
Deleted: pkg/trunk/calibration/stack.xml
===================================================================
--- pkg/trunk/calibration/stack.xml 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/stack.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,25 +0,0 @@
-<stack name="calibration" version="0.1">
- <description brief="calibration packages from ros-pkg">
- These are calibration-related packages.
- </description>
- <author>Vijay Pradeep vpr...@wi...</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/calibration</url>
-
- <depend stack="controllers"/> <!-- pr2_mechanism_controllers -->
- <depend stack="opencv"/> <!-- opencv_latest -->
- <depend stack="geometry"/> <!-- kdl, tf -->
- <depend stack="drivers"/> <!-- dense_laser_assembler, phase_space -->
- <depend stack="mechanism"/> <!-- mechanism_model, hardware_interface, mechanism_msgs -->
- <depend stack="util"/> <!-- topic_synchronizer -->
-
- <depend stack="common"/> <!-- tinyxml, robot_actions, laser_scan, manipulation_msgs -->
- <depend stack="ros"/>
- <depend stack="common_msgs"/> <!-- geometry_msgs, sensor_msgs, diagnostic_msgs -->
- <depend stack="visual_feature_detectors"/> <!-- checkerboard_detector -->
- <depend stack="stereo" /> <!-- stereo_msgs -->
- <depend stack="pr2_common" />
-
- <!--depend stack="openrave_planning"/--> <!-- openraveros -->
-</stack>
Copied: pkg/trunk/stacks/calibration/calibration_msgs/CMakeLists.txt (from rev 22856, pkg/trunk/calibration/calibration_msgs/CMakeLists.txt)
===================================================================
--- pkg/trunk/stacks/calibration/calibration_msgs/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/calibration_msgs/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
Copied: pkg/trunk/stacks/calibration/calibration_msgs/Makefile (from rev 22856, pkg/trunk/calibration/calibration_msgs/Makefile)
===================================================================
--- pkg/trunk/stacks/calibration/calibration_msgs/Makefile (rev 0)
+++ pkg/trunk/stacks/calibration/calibration_msgs/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml (from rev 22856, pkg/trunk/calibration/calibration_msgs/manifest.xml)
===================================================================
--- pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml (rev 0)
+++ pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,14 @@
+<package>
+ <description brief="calibration_msgs">
+
+ calibration_msgs
+
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/calibration_msgs</url>
+
+</package>
+
+
Copied: pkg/trunk/stacks/calibration/calibration_msgs/msg/Interval.msg (from rev 22856, pkg/trunk/calibration/calibration_msgs/msg/Interval.msg)
===================================================================
--- pkg/trunk/stacks/calibration/calibration_msgs/msg/Interval.msg (rev 0)
+++ pkg/trunk/stacks/calibration/calibration_msgs/msg/Interval.msg 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,2 @@
+time start
+time end
Copied: pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt (from rev 22856, pkg/trunk/calibration/joint_states_settler/CMakeLists.txt)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Copied: pkg/trunk/stacks/calibration/joint_states_settler/Makefile (from rev 22856, pkg/trunk/calibration/joint_states_settler/Makefile)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/Makefile (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml (from rev 22856, pkg/trunk/calibration/joint_states_settler/manifest.xml)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,17 @@
+<package>
+ <description brief="joint_states_settler">
+
+ joint_states_settler
+
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/joint_states_settler</url>
+ <depend package="roscpp"/>
+ <depend package="actionlib"/>
+ <depend package="mechanism_msgs"/>
+
+</package>
+
+
Copied: pkg/trunk/stacks/calibration/settlerlib/CMakeLists.txt (from rev 22856, pkg/trunk/calibration/settlerlib/CMakeLists.txt)
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/settlerlib/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+add_subdirectory(test EXCLUDE_FROM_ALL)
Copied: pkg/trunk/stacks/calibration/settlerlib/Makefile (from rev 22856, pkg/trunk/calibration/settlerlib/Makefile)
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/Makefile (rev 0)
+++ pkg/trunk/stacks/calibration/settlerlib/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h (from rev 22856, pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h)
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h (rev 0)
+++ pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,56 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef SETTLERLIB_DEFLATED_H_
+#define SETTLERLIB_DEFLATED_H_
+
+#include <boost/shared_ptr.hpp>
+#include "roslib/Header.h"
+
+namespace settlerlib
+{
+
+class Deflated
+{
+public:
+ roslib::Header header; // Need header to put in cache
+ std::vector<double> channel_;
+};
+
+}
+
+#endif
+
+
+
Copied: pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/sorted_deque.h (from rev 22856, pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h)
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/sorted_deque.h (rev 0)
+++ pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/sorted_deque.h 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,337 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <deque>
+#include <boost/shared_ptr.hpp>
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
+#include "ros/time.h"
+#include "ros/console.h"
+
+#ifndef SETTLERLIB_SORTED_DEQUE_H_
+#define SETTLERLIB_SORTED_DEQUE_H_
+
+#define DEQUE_DEBUG(fmt, ...) \
+ ROS_DEBUG_NAMED(logger_.c_str(), fmt,##__VA_ARGS__)
+
+namespace settlerlib
+{
+
+/**
+ * \brief Adds helper routines to the STL Deque for holding messages with headers
+ *
+ * Provides functionality to store a deque of messages that are sorted by timestamp. Users can
+ * then extract specific intervals of messages. Old messages fall off the back of the deque
+ */
+template <class M>
+class SortedDeque : public std::deque<M>
+{
+public:
+
+ using std::deque<M>::size;
+ using std::deque<M>::front;
+ using std::deque<M>::pop_front;
+ using std::deque<M>::begin;
+ using std::deque<M>::end;
+ using std::deque<M>::rbegin;
+ using std::deque<M>::rend;
+ using std::deque<M>::insert;
+ using std::deque<M>::at;
+ using std::deque<M>::erase;
+
+ /**
+ * \brief Assumes that '.header.stamp' can be used to get the stamp used for sorting
+ * \param logger name of rosconsole logger to display debugging output from this deque
+ */
+ SortedDeque(std::string logger = "deque") : std::deque<M>(), logger_(logger)
+ {
+ getStamp = &SortedDeque<M>::getStructStamp;
+ }
+
+ /**
+ * \brief Advanced constructor, allowing user to specific the timestamp getter method
+ * \param getStampFunc Function pointer specific how to get timestamp from the contained datatype
+ * \param logger name of rosconsole logger to display debugging output from this deque
+ */
+ SortedDeque(boost::function<const ros::Time&(const M&)> getStampFunc,
+ std::string logger = "deque") : std::deque<M>(), logger_(logger)
+ {
+ getStamp = getStampFunc;
+ }
+
+ /**
+ * \brief Set the maximum # of elements this deque can hold. Older elems are popped once the length is exeeded
+ * \param max_size The maximum # of elems to hold. Passing in 0 implies that the queue can grow indefinitely (which is a recipe for a memory leak)
+ */
+ void setMaxSize(unsigned int max_size)
+ {
+ max_size_ = max_size ;
+ }
+
+ /**
+ * \brief Add a new element to the deque, correctly sorted by timestamp
+ * \param msg the element to add
+ */
+ void add(const M& msg)
+ {
+ DEQUE_DEBUG("Called add()");
+ DEQUE_DEBUG_STATS(" ");
+ if (max_size_ != 0)
+ {
+ while (size() >= max_size_) // Keep popping off old data until we have space for a new msg
+ {
+ pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
+ DEQUE_DEBUG(" Popping element");
+ DEQUE_DEBUG_STATS(" ");
+ }
+ }
+
+ typename std::deque<M>::reverse_iterator rev_it = rbegin();
+
+ // Keep walking backwards along deque until we hit the beginning,
+ // or until we find a timestamp that's smaller than (or equal to) msg's timestamp
+ while(rev_it != rend() && getStamp(*rev_it) > getStamp(msg))
+ rev_it++;
+
+ // Add msg to the cache
+ insert(rev_it.base(), msg);
+ DEQUE_DEBUG(" Done inserting");
+ DEQUE_DEBUG_STATS(" ");
+ }
+
+ /**
+ * \brief Extract all the elements that occur in the interval between the start and end times
+ * \param start The start of the interval
+ * \param end The end of the interval
+ */
+ std::vector<M> getInterval(const ros::Time& start, const ros::Time& end)
+ {
+ // Find the starting index. (Find the first index after [or at] the start of the interval)
+ unsigned int start_index = 0 ;
+ while(start_index < size() &&
+ getStamp(at(start_index)) < start)
+ {
+ start_index++ ;
+ }
+
+ // Find the ending index. (Find the first index after the end of interval)
+ unsigned int end_index = start_index ;
+ while(end_index < size() &&
+ getStamp(at(end_index)) <= end)
+ {
+ end_index++ ;
+ }
+
+ std::vector<M> interval_elems ;
+ interval_elems.reserve(end_index - start_index) ;
+ for (unsigned int i=start_index; i<end_index; i++)
+ {
+ interval_elems.push_back(at(i)) ;
+ }
+
+ return interval_elems ;
+ }
+
+ /**
+ * Retrieve the smallest interval of messages that surrounds an interval from start to end.
+ * If the messages in the cache do not surround (start,end), then this will return the interval
+ * that gets closest to surrounding (start,end)
+ */
+ std::vector<M> getSurroundingInterval(const ros::Time& start, const ros::Time& end)
+ {
+ // Find the starting index. (Find the first index after [or at] the start of the interval)
+ unsigned int start_index = size()-1;
+ while(start_index > 0 &&
+ getStamp(at(start_index)) > start)
+ {
+ start_index--;
+ }
+ unsigned int end_index = start_index;
+ while(end_index < size()-1 &&
+ getStamp(at(end_index)) < end)
+ {
+ end_index++;
+ }
+
+ std::vector<M> interval_elems;
+ interval_elems.reserve(end_index - start_index + 1) ;
+ for (unsigned int i=start_index; i<=end_index; i++)
+ {
+ interval_elems.push_back(at(i)) ;
+ }
+
+ return interval_elems;
+ }
+
+ /**
+ * \brief Grab the oldest element that occurs right before the specified time.
+ * \param time The time that must occur after the elem
+ * \param out Output: Stores the extracted elem
+ * \return False there are no elems before the specified time
+ **/
+ bool getElemBeforeTime(const ros::Time& time, M& out)
+ {
+ unsigned int i=0 ;
+ int elem_index = -1 ;
+ while (i<size() &&
+ getStamp(at(i)) < time)
+ {
+ elem_index = i ;
+ i++ ;
+ }
+
+ if (elem_index >= 0)
+ {
+ out = at(elem_index);
+ return true;
+ }
+ //out = M();
+ return false;
+ }
+
+ /**
+ * \brief Grab the oldest element that occurs right after the specified time.
+ * \param time The time that must occur before the elem
+ * \param out Output: Stores the extracted elem
+ * \return False there are no elems after the specified time
+ */
+ bool getElemAfterTime(const ros::Time& time, M& out)
+ {
+ int i=size()-1 ;
+ int elem_index = -1 ;
+ while (i>=0 &&
+ getStamp(at(i)) > time)
+ {
+ elem_index = i ;
+ i-- ;
+ }
+
+ if (elem_index >= 0)
+ {
+ out = at(elem_index);
+ return true;
+ }
+ //out = M();
+ return false;
+ }
+
+ /**
+ * \brief Get the elem that occurs closest to the specified time
+ * \param time The time that we want to closest elem to
+ * \param out Output: Stores the extracted elem
+ * \return False if the queue is empty
+ */
+ bool getClosestElem(const ros::Time& time, M& out)
+ {
+ if (size() == 0)
+ return false;
+
+ typename std::deque<M>::iterator it = begin();
+ typename std::deque<M>::iterator best = it;
+
+ double best_diff = fabs( (time - getStamp(*best)).toSec());
+
+ while (it != end())
+ {
+ double cur_diff = fabs( (time - getStamp(*it)).toSec());
+ if (cur_diff < best_diff)
+ {
+ best_diff = cur_diff;
+ best = it;
+ }
+ ++it;
+ }
+ out = *best;
+ return true;
+ }
+
+
+ /**
+ * \brief Removes all elements that occur before the specified time
+ * \param time All elems that occur before this are removed from the deque
+ */
+ void removeAllBeforeTime(const ros::Time& time)
+ {
+ DEQUE_DEBUG("Called removeAllBeforeTime()");
+ DEQUE_DEBUG(" Erasing all elems before time: %u %u", time.sec, time.nsec);
+ typename std::deque<M>::iterator it = begin();
+
+ while (size() > 0 && getStamp(front()) < time)
+ {
+ DEQUE_DEBUG(" Erasing elem at time: %u, %u", getStamp(front()).sec, getStamp(front()).nsec);
+ pop_front();
+ DEQUE_DEBUG(" Erased an elem");
+ DEQUE_DEBUG_STATS(" ");
+ }
+ DEQUE_DEBUG(" Done erasing elems");
+ }
+
+ static const ros::Time& getPtrStamp(const M& m)
+ {
+ return m->header.stamp;
+ }
+
+ static const ros::Time& getStructStamp(const M& m)
+ {
+ return m.header.stamp;
+ }
+
+ static const ros::Time& getHeaderStamp(const M& m)
+ {
+ return m.stamp;
+ }
+private:
+ unsigned int max_size_;
+ std::string logger_;
+
+
+ /*const ros::Time& getStamp(const M& m)
+ {
+ return getStructStamp(m);
+ }*/
+
+ boost::function<const ros::Time&(const M&)> getStamp;
+
+ inline void DEQUE_DEBUG_STATS(const std::string& prefix)
+ {
+ DEQUE_DEBUG("%sdeque.size(): %u max_size: %u", prefix.c_str(), (unsigned int) size(), max_size_);
+ }
+};
+
+}
+
+#undef DEQUE_DEBUG
+#undef DEQUE_WARN
+
+#endif
Copied: pkg/trunk/stacks/calibration/settlerlib/manifest.xml (from rev 22856, pkg/trunk/calibration/settlerlib/manifest.xml)
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/manifest.xml (rev 0)
+++ pkg/trunk/stacks/calibration/settlerlib/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,14 @@
+<package>
+ <description brief="A set of routines to help with creating a settler">
+ Defines helper functions and routines that greatly help when trying to create a settler
+ for a specific sensor channel.
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/settlerlib</url>
+ <depend package="roscpp"/>
+
+</package>
+
+
Copied: pkg/trunk/stacks/calibration/settlerlib/test/CMakeLists.txt (from rev 22856, pkg/trunk/calibration/settlerlib/test/CMakeLists.txt)
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/test/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/settlerlib/test/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,5 @@
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
+
+# ********** Tests **********
+
+rospack_add_gtest(test/sorted_deque_unittest sorted_deque_unittest.cpp)
Copied: pkg/trunk/stacks/calibration/settlerlib/test/sorted_deque_unittest.cpp (from rev 22856, pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/test/sorted_deque_unittest.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/settlerlib/test/sorted_deque_unittest.cpp 2009-08-25 17:41:58 UTC (rev 22859)
@@ -0,0 +1,222 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+#include <gtest/gtest.h>
+
+#include "settlerlib/sorted_deque.h"
+
+using namespace std;
+using namespace settlerlib;
+
+struct Header
+{
+ ros::Time stamp ;
+} ;
+
+
+struct Msg
+{
+ Header header ;
+ int data ;
+} ;
+
+void fillEasy(SortedDeque<Msg>& sd, unsigned int start, unsigned int end)
+{
+ for (unsigned int i=start; i < end; i++)
+ {
+ Msg msg ;
+ msg.data = i ;
+ msg.header.stamp.fromSec(i*10) ;
+
+ sd.add(msg) ;
+ }
+}
+
+TEST(SortedDeque, easyInterval)
+{
+ SortedDeque<Msg> sd;
+ sd.setMaxSize(10);
+
+ fillEasy(sd, 0, 5);
+
+ vector<Msg> interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
+
+ EXPECT_EQ(interval_data.size(), (unsigned int) 3) ;
+ EXPECT_EQ(interval_data[0].data, 1) ;
+ EXPECT_EQ(interval_data[1].data, 2) ;
+ EXPECT_EQ(interval_data[2].data, 3) ;
+
+ // Look for an interval past the end of the cache
+ interval_data = sd.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
+ EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
+
+ // Look for an interval that fell off the back of the cache
+ fillEasy(sd, 5, 20) ;
+ interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
+ EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
+}
+
+TEST(SortedDeque, easySurroundingInterval)
+{
+ SortedDeque<Msg> cache;
+ cache.setMaxSize(10);
+ fillEasy(cache, 1, 6);
+
+ vector<Msg> interval_data;
+ interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
+ EXPECT_EQ(interval_data.size(), (unsigned int) 4);
+ EXPECT_EQ(interval_data[0].data, 1);
+ EXPECT_EQ(interval_data[1].data, 2);
+ EXPECT_EQ(interval_data[2].data, 3);
+ EXPECT_EQ(interval_data[3].data, 4);
+
+ interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
+ EXPECT_EQ(interval_data.size(), (unsigned int) 4);
+ EXPECT_EQ(interval_data[0].data, 1);
+
+ interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
+ EXPECT_EQ(interval_data.size(), (unsigned int) 2);
+ EXPECT_EQ(interval_data[0].data, 3);
+ EXPECT_EQ(interval_data[1].data, 4);
+
+ interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
+ EXPECT_EQ(interval_data.size(), (unsigned int) 1);
+ EXPECT_EQ(interval_data[0].data, 5);
+}
+
+Msg buildMsg(double time, int data)
+{
+ Msg msg;
+ msg.data = data;
+ msg.header.stamp....
[truncated message content] |
