Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23200] pkg/trunk
|
From: <vij...@us...> - 2009-08-27 23:53:12
|
Revision: 23200
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23200&view=rev
Author: vijaypradeep
Date: 2009-08-27 23:52:59 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Adding monocam_settler along with unit tests. Also add monocam_settler_node.
Modified Paths:
--------------
pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/calibration/calibration_msgs/msg/ImageObjectFeatures.msg
pkg/trunk/stacks/calibration/calibration_msgs/msg/ImagePoint.msg
pkg/trunk/stacks/calibration/monocam_settler/
pkg/trunk/stacks/calibration/monocam_settler/CMakeLists.txt
pkg/trunk/stacks/calibration/monocam_settler/Makefile
pkg/trunk/stacks/calibration/monocam_settler/action/
pkg/trunk/stacks/calibration/monocam_settler/action/Config.action
pkg/trunk/stacks/calibration/monocam_settler/include/
pkg/trunk/stacks/calibration/monocam_settler/include/monocam_settler/
pkg/trunk/stacks/calibration/monocam_settler/include/monocam_settler/monocam_settler.h
pkg/trunk/stacks/calibration/monocam_settler/manifest.xml
pkg/trunk/stacks/calibration/monocam_settler/msg/
pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigAction.msg
pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionFeedback.msg
pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionGoal.msg
pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionResult.msg
pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigFeedback.msg
pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigGoal.msg
pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigResult.msg
pkg/trunk/stacks/calibration/monocam_settler/src/
pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler.cpp
pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler_node.cpp
pkg/trunk/stacks/calibration/monocam_settler/test/
pkg/trunk/stacks/calibration/monocam_settler/test/CMakeLists.txt
pkg/trunk/stacks/calibration/monocam_settler/test/monocam_settler_unittest.cpp
Removed Paths:
-------------
pkg/trunk/calibration_experimental/experimental_calibration_msgs/msg/ImagePoint.msg
Deleted: pkg/trunk/calibration_experimental/experimental_calibration_msgs/msg/ImagePoint.msg
===================================================================
--- pkg/trunk/calibration_experimental/experimental_calibration_msgs/msg/ImagePoint.msg 2009-08-27 23:47:50 UTC (rev 23199)
+++ pkg/trunk/calibration_experimental/experimental_calibration_msgs/msg/ImagePoint.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -1,3 +0,0 @@
-# Stores the xy location of a point in an image in pixel coordinates
-float64 x
-float64 y
Modified: pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml 2009-08-27 23:47:50 UTC (rev 23199)
+++ pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml 2009-08-27 23:52:59 UTC (rev 23200)
@@ -9,6 +9,9 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/calibration_msgs</url>
+ <!-- common_msgs -->
+ <depend package="geometry_msgs" />
+
<export>
<cpp cflags="-I${prefix}/msg/cpp" />
</export>
Added: pkg/trunk/stacks/calibration/calibration_msgs/msg/ImageObjectFeatures.msg
===================================================================
--- pkg/trunk/stacks/calibration/calibration_msgs/msg/ImageObjectFeatures.msg (rev 0)
+++ pkg/trunk/stacks/calibration/calibration_msgs/msg/ImageObjectFeatures.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,11 @@
+# Synchronized with sensor output
+Header header
+
+# Pixel locations of detected features
+ImagePoint[] image_points
+
+# Defines geometry of detected features in some "object" coordinate frame
+geometry_msgs/Point[] object_points
+
+# False on detection failure or partial detection
+uint8 success
Copied: pkg/trunk/stacks/calibration/calibration_msgs/msg/ImagePoint.msg (from rev 23199, pkg/trunk/calibration_experimental/experimental_calibration_msgs/msg/ImagePoint.msg)
===================================================================
--- pkg/trunk/stacks/calibration/calibration_msgs/msg/ImagePoint.msg (rev 0)
+++ pkg/trunk/stacks/calibration/calibration_msgs/msg/ImagePoint.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,3 @@
+# Stores the xy location of a point in an image in pixel coordinates
+float64 x
+float64 y
Added: pkg/trunk/stacks/calibration/monocam_settler/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/CMakeLists.txt 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,33 @@
+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(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+rosbuild_genmsg()
+# rosbuild_gensrv()
+
+rosbuild_add_library(${PROJECT_NAME} src/monocam_settler.cpp)
+
+rosbuild_add_executable(monocam_settler_node src/monocam_settler_node.cpp)
+target_link_libraries(monocam_settler_node ${PROJECT_NAME})
+
+
+#common commands for building c++ executables and libraries
+#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)
Added: pkg/trunk/stacks/calibration/monocam_settler/Makefile
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/Makefile (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/Makefile 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/stacks/calibration/monocam_settler/action/Config.action
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/action/Config.action (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/action/Config.action 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,6 @@
+float64 tolerance # Tolerance on each of the features
+uint8 ignore_failures # True if we discard
+duration max_step # The maximum timestep between two elements in an interval
+uint32 cache_size # The size of our cache when searching for an interval
+---
+---
Added: pkg/trunk/stacks/calibration/monocam_settler/include/monocam_settler/monocam_settler.h
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/include/monocam_settler/monocam_settler.h (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/include/monocam_settler/monocam_settler.h 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,82 @@
+/*********************************************************************
+* 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 MONOCAM_SETTLER_MONOCAM_SETTLER_H_
+#define MONOCAM_SETTLER_MONOCAM_SETTLER_H_
+
+#include <boost/shared_ptr.hpp>
+#include <calibration_msgs/ImageObjectFeatures.h>
+#include <calibration_msgs/Interval.h>
+
+#include <settlerlib/sorted_deque.h>
+#include <settlerlib/deflated.h>
+
+#include <monocam_settler/ConfigGoal.h>
+
+namespace monocam_settler
+{
+
+class DeflatedImageObjectFeatures : public settlerlib::Deflated
+{
+public:
+ calibration_msgs::ImageObjectFeaturesConstPtr msg_;
+};
+
+
+class MonocamSettler
+{
+public:
+ MonocamSettler();
+ bool configure(const monocam_settler::ConfigGoal& goal);
+
+ bool add(const calibration_msgs::ImageObjectFeaturesConstPtr msg,
+ calibration_msgs::Interval& interval);
+
+private:
+ bool configured_;
+ double tol_;
+ ros::Duration max_step_;
+ bool ignore_failures_;
+
+ void deflate(const calibration_msgs::ImageObjectFeaturesConstPtr& image_features,
+ DeflatedImageObjectFeatures& deflated);
+
+ typedef settlerlib::SortedDeque< boost::shared_ptr<const DeflatedImageObjectFeatures> > DeflatedMsgCache;
+ typedef settlerlib::SortedDeque<settlerlib::DeflatedConstPtr> DeflatedCache;
+ DeflatedMsgCache cache_;
+};
+
+}
+
+#endif
Added: pkg/trunk/stacks/calibration/monocam_settler/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/manifest.xml (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/manifest.xml 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,21 @@
+<package>
+ <description brief="Waits for features in a single camera to settler">
+ Listens on a ImageFeatures topic, and waits for the data to settle
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/monocam_settler</url>
+
+ <!-- ros -->
+ <depend package="roscpp"/>
+
+ <!-- common_msgs -->
+ <depend package="actionlib_msgs" />
+
+ <depend package="settlerlib"/>
+ <depend package="calibration_msgs"/>
+
+</package>
+
+
Added: pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigAction.msg
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigAction.msg (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigAction.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,4 @@
+
+ConfigActionGoal action_goal
+ConfigActionResult action_result
+ConfigActionFeedback action_feedback
Added: pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionFeedback.msg
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionFeedback.msg (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionFeedback.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalStatus status
+ConfigFeedback feedback
Added: pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionGoal.msg
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionGoal.msg (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionGoal.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalID goal_id
+ConfigGoal goal
Added: pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionResult.msg
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionResult.msg (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigActionResult.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib_msgs/GoalStatus status
+ConfigResult result
Added: pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigFeedback.msg
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigFeedback.msg (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigFeedback.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1 @@
+
Added: pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigGoal.msg
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigGoal.msg (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/msg/ConfigGoal.msg 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,4 @@
+float64 tolerance # Tolerance on each of the features
+uint8 ignore_failures # True if we discard
+duration max_step # The maximum timestep between two elements in an interval
+uint32 cache_size # The size of our cache when searching for an interval
Added: pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler.cpp
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler.cpp 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,106 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include <monocam_settler/monocam_settler.h>
+#include <settlerlib/interval_calc.h>
+
+using namespace monocam_settler;
+
+
+MonocamSettler::MonocamSettler() : cache_(&DeflatedMsgCache::getPtrStamp)
+{
+ configured_ = false;
+}
+
+bool MonocamSettler::configure(const monocam_settler::ConfigGoal& goal)
+{
+ tol_ = goal.tolerance;
+ max_step_ = goal.max_step;
+ ignore_failures_ = goal.ignore_failures;
+ cache_.clear();
+ cache_.setMaxSize(goal.cache_size);
+
+ ROS_DEBUG("Configuring MonocamSettler with tolerance of [%.3f]", tol_);
+
+ configured_ = true;
+ return true;
+}
+
+
+bool MonocamSettler::add(const calibration_msgs::ImageObjectFeaturesConstPtr msg,
+ calibration_msgs::Interval& interval)
+{
+ if (!configured_)
+ {
+ ROS_WARN("Not configured. Going to skip");
+ return false;
+ }
+
+ // Check if detection failed
+ if (!msg->success)
+ {
+ if(!ignore_failures_) // If we care about failures then we should reset the cache
+ cache_.clear();
+ return false;
+ }
+
+ boost::shared_ptr<DeflatedImageObjectFeatures> deflated(new DeflatedImageObjectFeatures);
+ deflate(msg, *deflated);
+ cache_.add(deflated);
+
+ DeflatedCache* bare_cache = (DeflatedCache*)(&cache_);
+
+ std::vector<double> tol_vec(deflated->channels_.size(), tol_);
+
+ interval = settlerlib::IntervalCalc::computeLatestInterval(*bare_cache, tol_vec, max_step_);
+
+ return true;
+}
+
+void MonocamSettler::deflate(const calibration_msgs::ImageObjectFeaturesConstPtr& msg,
+ DeflatedImageObjectFeatures& deflated)
+{
+ deflated.header.stamp = msg->header.stamp;
+
+ const unsigned int N = msg->image_points.size();
+ deflated.channels_.resize( 2 * N );
+ for (unsigned int i=0; i<N; i++)
+ {
+ deflated.channels_[2*i+0] = msg->image_points[i].x;
+ deflated.channels_[2*i+1] = msg->image_points[i].y;
+ }
+ deflated.msg_ = msg;
+}
Added: pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler_node.cpp
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler_node.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/src/monocam_settler_node.cpp 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,102 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include <ros/ros.h>
+#include <monocam_settler/monocam_settler.h>
+
+using namespace monocam_settler;
+
+monocam_settler::ConfigGoal getParamConfig(ros::NodeHandle& n)
+{
+ monocam_settler::ConfigGoal config;
+
+ n.param("~tolerance", config.tolerance, 1.0);
+ ROS_INFO("tolerance: %.3f", config.tolerance);
+
+ bool ignore_failures;
+ n.param("~ignore_failures", ignore_failures, true);
+ config.ignore_failures = ignore_failures;
+ if (config.ignore_failures)
+ ROS_INFO("Ignore Failures: True");
+ else
+ ROS_INFO("Ignore Failures: False");
+
+ double max_step;
+ n.param("~max_step", max_step, 1000.0);
+ config.max_step = ros::Duration(max_step);
+ ROS_INFO("max step: [%.3fs]", config.max_step.toSec());
+
+ int cache_size;
+ n.param("~cache_size", cache_size, 1000);
+ if (cache_size < 0)
+ ROS_FATAL("cache_size < 0. (cache_size==%i)", cache_size);
+ config.cache_size = (unsigned int) cache_size;
+ ROS_INFO("cache_size: [%u]", config.cache_size);
+
+ return config;
+}
+
+void msgCallback(ros::Publisher* pub, MonocamSettler* settler, const calibration_msgs::ImageObjectFeaturesConstPtr& msg)
+{
+ bool success;
+
+ calibration_msgs::Interval interval;
+ success = settler->add(msg, interval);
+
+ if (success)
+ pub->publish(interval);
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "monocam_settler");
+
+ ros::NodeHandle n;
+
+ // Set up the MonocamSettler
+ monocam_settler::ConfigGoal config = getParamConfig(n);
+ MonocamSettler settler;
+ settler.configure(config);
+
+ // Output
+ ros::Publisher pub = n.advertise<calibration_msgs::Interval>("settled", 1);
+
+ // Input
+ boost::function<void (const calibration_msgs::ImageObjectFeaturesConstPtr&)> cb = boost::bind(&msgCallback, &pub, &settler, _1);
+ ros::Subscriber sub = n.subscribe(std::string("features"), 1, cb);
+
+ ros::spin();
+}
Added: pkg/trunk/stacks/calibration/monocam_settler/test/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/test/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/test/CMakeLists.txt 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,5 @@
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
+
+# ********** Tests **********
+rospack_add_gtest(test/monocam_settler_unittest monocam_settler_unittest.cpp)
+target_link_libraries(test/monocam_settler_unittest monocam_settler)
Added: pkg/trunk/stacks/calibration/monocam_settler/test/monocam_settler_unittest.cpp
===================================================================
--- pkg/trunk/stacks/calibration/monocam_settler/test/monocam_settler_unittest.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/monocam_settler/test/monocam_settler_unittest.cpp 2009-08-27 23:52:59 UTC (rev 23200)
@@ -0,0 +1,290 @@
+/*********************************************************************
+* 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 <monocam_settler/ConfigGoal.h>
+#include <monocam_settler/monocam_settler.h>
+
+using namespace std;
+using namespace monocam_settler;
+
+
+
+static const unsigned int N = 9;
+static const unsigned int C = 2;
+
+static const double data[N][2*C] = { { 0, 0, 0, 0 },
+ { 1, 2, 1, 1 },
+ { 2, 4, 0, 0 },
+ { 3, 6, 1, 1 },
+ { 4, 8, 0, 0 },
+ { 0, 0, 1, 1 },
+ { 4, 1, 0, 0 },
+ { 8, 2, 1, 1 },
+ { 12, 3, 0, 0 }};
+
+// 2 possible sets of timestamps for the data
+static const unsigned int times[N][2] = { { 0, 0 },
+ { 1, 1 },
+ { 2, 2 },
+ { 3, 3 },
+ { 4, 4 },
+ { 5, 15 },
+ { 6, 16 },
+ { 7, 17 },
+ { 8, 18 } };
+
+static const bool success[N] = { true,
+ true,
+ false,
+ true,
+ true,
+ true,
+ true,
+ true,
+ true };
+
+// add data to the settler, and see what intervals come out
+vector<calibration_msgs::Interval> addToSettler(MonocamSettler& settler, unsigned int time_channel, bool always_success)
+{
+ vector<calibration_msgs::Interval> intervals;
+
+ for (unsigned int i=0; i<N; i++)
+ {
+ calibration_msgs::ImageObjectFeaturesPtr msg(new calibration_msgs::ImageObjectFeatures);
+ msg->header.stamp = ros::Time(times[i][time_channel], 0);
+
+ msg->image_points.resize(C);
+ for (unsigned int j=0; j<C; j++)
+ {
+ msg->image_points[j].x = data[i][2*j+0];
+ msg->image_points[j].y = data[i][2*j+1];
+ }
+ msg->success = always_success || success[i];
+
+ calibration_msgs::Interval cur_interval;
+ bool result = settler.add(msg, cur_interval);
+ if (result)
+ intervals.push_back(cur_interval);
+ else
+ intervals.push_back(calibration_msgs::Interval());
+ }
+
+ return intervals;
+}
+
+void doEasyCheck(const vector<calibration_msgs::Interval>& intervals)
+{
+ ASSERT_EQ(intervals.size(), N);
+ EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
+
+ EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
+
+ EXPECT_EQ(intervals[2].start.sec, (unsigned int) 1);
+ EXPECT_EQ(intervals[2].end.sec, (unsigned int) 2);
+
+ EXPECT_EQ(intervals[3].start.sec, (unsigned int) 2);
+ EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
+
+ EXPECT_EQ(intervals[4].start.sec, (unsigned int) 3);
+ EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
+
+ EXPECT_EQ(intervals[5].start.sec, (unsigned int) 5);
+ EXPECT_EQ(intervals[5].end.sec, (unsigned int) 5);
+
+ EXPECT_EQ(intervals[6].start.sec, (unsigned int) 6);
+ EXPECT_EQ(intervals[6].end.sec, (unsigned int) 6);
+
+ EXPECT_EQ(intervals[7].start.sec, (unsigned int) 7);
+ EXPECT_EQ(intervals[7].end.sec, (unsigned int) 7);
+
+ EXPECT_EQ(intervals[8].start.sec, (unsigned int) 8);
+ EXPECT_EQ(intervals[8].end.sec, (unsigned int) 8);
+}
+
+void doIgnoreFailuresCheck(const vector<calibration_msgs::Interval>& intervals)
+{
+ ASSERT_EQ(intervals.size(), N);
+ EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
+
+ EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
+
+ EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[2].end.sec, (unsigned int) 0);
+
+ EXPECT_EQ(intervals[3].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
+
+ EXPECT_EQ(intervals[4].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
+
+ EXPECT_EQ(intervals[5].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[5].end.sec, (unsigned int) 5);
+
+ EXPECT_EQ(intervals[6].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[6].end.sec, (unsigned int) 6);
+
+ EXPECT_EQ(intervals[7].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[7].end.sec, (unsigned int) 7);
+
+ EXPECT_EQ(intervals[8].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[8].end.sec, (unsigned int) 8);
+}
+
+void doCatchFailuresCheck(const vector<calibration_msgs::Interval>& intervals)
+{
+ ASSERT_EQ(intervals.size(), N);
+ EXPECT_EQ(intervals[0].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[0].end.sec, (unsigned int) 0);
+
+ EXPECT_EQ(intervals[1].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[1].end.sec, (unsigned int) 1);
+
+ EXPECT_EQ(intervals[2].start.sec, (unsigned int) 0);
+ EXPECT_EQ(intervals[2].end.sec, (unsigned int) 0);
+
+ EXPECT_EQ(intervals[3].start.sec, (unsigned int) 3);
+ EXPECT_EQ(intervals[3].end.sec, (unsigned int) 3);
+
+ EXPECT_EQ(intervals[4].start.sec, (unsigned int) 3);
+ EXPECT_EQ(intervals[4].end.sec, (unsigned int) 4);
+
+ EXPECT_EQ(intervals[5].start.sec, (unsigned int) 3);
+ EXPECT_EQ(intervals[5].end.sec, (unsigned int) 5);
+
+ EXPECT_EQ(intervals[6].start.sec, (unsigned int) 3);
+ EXPECT_EQ(intervals[6].end.sec, (unsigned int) 6);
+
+ EXPECT_EQ(intervals[7].start.sec, (unsigned int) 3);
+ EXPECT_EQ(intervals[7].end.sec, (unsigned int) 7);
+
+ EXPECT_EQ(intervals[8].start.sec, (unsigned int) 3);
+ EXPECT_EQ(intervals[8].end.sec, (unsigned int) 8);
+}
+
+ConfigGoal config1()
+{
+ ConfigGoal config;
+
+ config.tolerance = 2.5;
+
+ config.ignore_failures = true;
+
+ config.max_step = ros::Duration(2,0);
+
+ config.cache_size = 100;
+
+ return config;
+}
+
+ConfigGoal config2(bool ignore_failures)
+{
+ ConfigGoal config;
+
+ config.tolerance = 100.0;
+
+ config.ignore_failures = ignore_failures;
+
+ config.max_step = ros::Duration(2,0);
+
+ config.cache_size = 100;
+
+ return config;
+}
+
+TEST(MonocamSettler, easyCheck)
+{
+ MonocamSettler settler;
+
+ bool config_result = settler.configure(config1());
+ ASSERT_TRUE(config_result);
+
+ vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, true);
+
+ doEasyCheck(intervals);
+}
+
+TEST(MonocamSettler, ignoreFailuresCheck)
+{
+ MonocamSettler settler;
+
+ bool config_result = settler.configure(config2(true));
+ ASSERT_TRUE(config_result);
+
+ vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, false);
+
+ doIgnoreFailuresCheck(intervals);
+}
+
+TEST(MonocamSettler, catchFailuresCheck)
+{
+ MonocamSettler settler;
+
+ bool config_result = settler.configure(config2(false));
+ ASSERT_TRUE(config_result);
+
+ vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, false);
+
+ doCatchFailuresCheck(intervals);
+}
+
+TEST(MonocamSettler, reconfigureCheck)
+{
+ MonocamSettler settler;
+
+ bool config_result = settler.configure(config1());
+ ASSERT_TRUE(config_result);
+
+ vector<calibration_msgs::Interval> intervals = addToSettler(settler, 0, true);
+
+ doEasyCheck(intervals);
+
+ config_result = settler.configure(config2(false));
+ ASSERT_TRUE(config_result);
+
+ intervals = addToSettler(settler, 0, false);
+
+ doCatchFailuresCheck(intervals);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
