Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20694] pkg/trunk
|
From: <vij...@us...> - 2009-08-04 20:59:38
|
Revision: 20694
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20694&view=rev
Author: vijaypradeep
Date: 2009-08-04 20:59:30 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Lots of additions for calibration capture pipeline. Also fixing led_detection build.
Modified Paths:
--------------
pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml
pkg/trunk/vision/led_detection/include/led_detection/led_detector.h
pkg/trunk/vision/led_detection/manifest.xml
pkg/trunk/vision/led_detection/src/led_detector.cpp
pkg/trunk/vision/led_detection/src/led_detector_node.cpp
Added Paths:
-----------
pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg
pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg
pkg/trunk/calibration/sandbox/calibration_message_filters/
pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt
pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile
pkg/trunk/calibration/sandbox/calibration_message_filters/include/
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h
pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h
pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml
pkg/trunk/calibration/sandbox/calibration_message_filters/test/
pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt
pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/capture_hand_led.h
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_hand_led.cpp
Removed Paths:
-------------
pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg
pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg
Copied: pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg (from rev 20685, pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg)
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/ImagePoint.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,3 @@
+# Stores the xy location of a point in an image in pixel coordinates
+float64 x
+float64 y
Copied: pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg (from rev 20685, pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg)
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/ImagePointStamped.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,2 @@
+Header header
+ImagePoint image_point
\ No newline at end of file
Deleted: pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -1,3 +0,0 @@
-# Stores the xy location of a point in an image in pixel coordinates
-float64 x
-float64 y
Deleted: pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg 2009-08-04 20:59:30 UTC (rev 20694)
@@ -1,2 +0,0 @@
-Header header
-ImagePoint image_point
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/CMakeLists.txt 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(calibration_message_filters)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_library(calibration_message_filters src/stationary_checker.cpp
+ src/joint_states_deflater.cpp)
+
+add_subdirectory(test EXCLUDE_FROM_ALL)
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/Makefile 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/deflated.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,64 @@
+/*********************************************************************
+* 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 CALIBRATION_MESSAGE_FILTERS_DEFLATED_H_
+#define CALIBRATION_MESSAGE_FILTERS_DEFLATED_H_
+
+#include <boost/shared_ptr.hpp>
+#include "roslib/Header.h"
+
+namespace calibration_message_filters
+{
+
+class Deflated
+{
+public:
+ roslib::Header header; // Need header to put in cache
+ std::vector<double> deflated_;
+};
+
+template <class M>
+class DeflatedMsg : public Deflated
+{
+public:
+ typedef boost::shared_ptr<const M> MConstPtr;
+ MConstPtr msg;
+};
+
+}
+
+#endif
+
+
+
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,67 @@
+/*********************************************************************
+* 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 CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
+#define CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
+
+#include "calibration_message_filters/deflated.h"
+#include "mechanism_msgs/JointStates.h"
+
+namespace calibration_message_filters
+{
+
+
+class JointStatesDeflater
+{
+public:
+ typedef DeflatedMsg<mechanism_msgs::JointStates> DeflatedJointStates;
+
+ JointStatesDeflater();
+
+ void setDeflationJointNames(std::vector<std::string> joint_names);
+ void deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem);
+
+private:
+ std::vector<unsigned int> mapping_;
+ std::vector<std::string> joint_names_;
+
+ void updateMapping(const mechanism_msgs::JointStates& joint_states);
+
+};
+
+}
+
+
+
+#endif
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,262 @@
+/*********************************************************************
+* 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 CALIBRATION_MESSAGE_FILTERS_SANDWICH_H_
+#define CALIBRATION_MESSAGE_FILTERS_SANDWICH_H_
+
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/signals.hpp>
+
+#include "message_filters/cache.h"
+
+#include "ros/time.h"
+#include "calibration_message_filters/sandwich_elem.h"
+
+namespace calibration_message_filters
+{
+
+template<class M, class S>
+class Sandwich
+{
+public:
+ typedef boost::shared_ptr<M const> MConstPtr;
+ typedef boost::shared_ptr<S const> SConstPtr;
+ typedef boost::function<void(const SandwichElem<M,S>&)> Callback;
+ typedef boost::signal<void(const SandwichElem<M,S>&)> Signal;
+
+ template<class F1>
+ Sandwich(F1& f1, message_filters::Cache<S>& cache, unsigned int queue_size=1,
+ const ros::Duration& padding_before=ros::Duration(0,0),
+ const ros::Duration& padding_after=ros::Duration(0,0),
+ const ros::Duration& expiration_limit=ros::Duration(10,0) )
+ {
+ setQueueSize(queue_size);
+ setPadding(padding_before, padding_after);
+ setExpirationLimit(expiration_limit);
+ connectTo(f1, cache);
+ }
+
+ Sandwich(unsigned int queue_size=1,
+ const ros::Duration& padding_before=ros::Duration(0,0),
+ const ros::Duration& padding_after=ros::Duration(0,0),
+ const ros::Duration& expiration_limit=ros::Duration(10,0) )
+ {
+ cache_ = NULL;
+ setQueueSize(queue_size);
+ setPadding(padding_before, padding_after);
+ setExpirationLimit(expiration_limit);
+ }
+
+ template<class F1>
+ void connectInput(F1& f1, message_filters::Cache<S>& cache)
+ {
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_msg_connection_ = f1.connect(boost::bind(&Sandwich::addToQueue, this, _1));
+ incoming_gate_connection_ = cache.connect(boost::bind(&Sandwich::addToGate, this, _1));
+ }
+
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+ cache_ = &cache;
+ }
+ }
+
+ void connectCacheInput(message_filters::Cache<S>& cache)
+ {
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_gate_connection_ = cache.registerCallback(boost::bind(&Sandwich::addToGate, this, _1));
+ }
+
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+ cache_ = &cache;
+ }
+ }
+
+ message_filters::Connection registerCallback(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return message_filters::Connection(boost::bind(&Sandwich::disconnect, this, _1), signal_.connect(callback));
+ }
+
+ ~Sandwich()
+ {
+ incoming_msg_connection_.disconnect();
+ incoming_gate_connection_.disconnect();
+ }
+
+ void setQueueSize(unsigned int queue_size)
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+
+ if (queue_size == 0)
+ return;
+
+ queue_size_ = queue_size;
+ }
+
+ void setPadding(const ros::Duration& padding_before,
+ const ros::Duration& padding_after)
+ {
+ padding_before_ = padding_before;
+ padding_after_ = padding_after;
+ }
+
+ void setExpirationLimit(const ros::Duration& expiration_limit)
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+ if (expiration_limit < ros::Duration(0,0))
+ return;
+ expiration_limit_ = expiration_limit;
+ }
+
+ void addToQueue(const MConstPtr& msg)
+ {
+
+ {
+ boost::mutex::scoped_lock lock(data_mutex_);
+
+ while (queue_.size() >= queue_size_) // Keep popping off old data until we have space for a new msg
+ queue_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
+
+ // Add the msg to the queue, sorted by header.stamp
+ typename std::deque<MConstPtr >::reverse_iterator rev_it = queue_.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 != queue_.rend() && (*rev_it)->header.stamp > msg->header.stamp)
+ rev_it++;
+ // Add msg to the cache
+ queue_.insert(rev_it.base(), msg);
+
+ }
+
+ processQueue();
+ }
+
+ void addToGate(const SConstPtr& msg)
+ {
+ processQueue();
+ }
+
+private:
+ void disconnect(const message_filters::Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
+ void processQueue()
+ {
+ std::vector<SandwichElem<M,S> > valid_elems;
+ {
+
+ boost::mutex::scoped_lock lock(data_mutex_);
+
+ if (!cache_)
+ return;
+
+ // Process all elems in the queue
+ typename std::deque<MConstPtr >::iterator it = queue_.begin();
+
+ while (it != queue_.end())
+ {
+ // Check if we need to throw away the elem
+ if ( (*it)->header.stamp - padding_before_ < cache_->getLatestTime() - expiration_limit_ )
+ {
+ it = queue_.erase(it);
+ }
+ else
+ {
+ // See what the interval looks like
+ std::vector<SConstPtr > interval_msgs = cache_->getSurroundingInterval((*it)->header.stamp - padding_before_,
+ (*it)->header.stamp + padding_after_);
+ if (interval_msgs.size() < 2)
+ {
+ ++it;
+ }
+ else if((*it)->header.stamp + padding_after_ < interval_msgs.back()->header.stamp &&
+ (*it)->header.stamp - padding_before_ > interval_msgs.front()->header.stamp)
+ {
+ SandwichElem<M,S> sandwich_elem;
+ sandwich_elem.data = *it;
+ sandwich_elem.interval = interval_msgs;
+ it = queue_.erase(it);
+ valid_elems.push_back(sandwich_elem);
+ }
+ else
+ {
+ ++it;
+ }
+ }
+ }
+ }
+
+ // Call the callback for each elem that was within range
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ for (unsigned int i=0; i<valid_elems.size(); i++)
+ signal_(valid_elems[i]) ;
+ }
+
+ }
+
+ boost::mutex signal_mutex_;
+ message_filters::Connection incoming_msg_connection_;
+ message_filters::Connection incoming_gate_connection_;
+ Signal signal_;
+
+ boost::mutex data_mutex_;
+ std::deque<MConstPtr > queue_;
+ message_filters::Cache<S>* cache_;
+ ros::Duration expiration_limit_;
+ unsigned int queue_size_;
+ ros::Duration padding_before_;
+ ros::Duration padding_after_;
+
+} ;
+
+
+
+
+
+
+
+
+}
+
+
+#endif
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/include/calibration_message_filters/sandwich_elem.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,60 @@
+/*********************************************************************
+* 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 CALIBRATION_MESSAGE_FILTERS_SANDWICH_ELEM_H_
+#define CALIBRATION_MESSAGE_FILTERS_SANDWICH_ELEM_H_
+
+namespace calibration_message_filters
+{
+
+#include <vector>
+#include "boost/shared_ptr.hpp"
+
+/**
+ * Output type for the sandwich filter. Stores some message type M,
+ * along with a vector of message type S, which 'sandwiches' M. Thus,
+ * the vector of S messages surrounds the message M, in time.
+ */
+
+template <class M, class S>
+struct SandwichElem
+{
+ boost::shared_ptr<const M> data;
+ std::vector< boost::shared_ptr<const S> > interval;
+};
+
+}
+
+
+#endif // CALIBRATION_MESSAGE_FILTERS_SANDWICH_ELEM_H_
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/manifest.xml 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,14 @@
+<package>
+ <description>Message filters that are especially useful for performing robot calibration tasks</description>
+ <author>Vijay Pradeep (vpr...@wi...)</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="roscpp" />
+ <depend package="message_filters" />
+ <depend package="mechanism_msgs" />
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lcalibration_message_filters" />
+ </export>
+ <url>http://pr.willowgarage.com/wiki/message_filters</url>
+</package>
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/test/CMakeLists.txt 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,7 @@
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
+
+# ********** Tests **********
+rospack_add_gtest(test/sandwich_unittest sandwich_unittest.cpp)
+
+rospack_add_gtest(test/joint_states_deflater_unittest joint_states_deflater_unittest.cpp)
+target_link_libraries(test/joint_states_deflater_unittest calibration_message_filters)
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/calibration_message_filters/test/sandwich_unittest.cpp 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,138 @@
+/*********************************************************************
+* 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 "ros/time.h"
+#include "message_filters/cache.h"
+#include "calibration_message_filters/sandwich.h"
+
+using namespace std ;
+using namespace message_filters ;
+using namespace calibration_message_filters ;
+
+struct Header
+{
+ ros::Time stamp ;
+} ;
+
+
+
+struct Msg
+{
+ Header header;
+ int data;
+} ;
+
+
+
+struct GateMsg
+{
+ Header header;
+ int junk1;
+ double junk2;
+} ;
+
+typedef boost::shared_ptr<const Msg> MsgConstPtr;
+typedef boost::shared_ptr<const GateMsg> GateMsgConstPtr;
+
+MsgConstPtr buildMsg(ros::Time time, int data)
+{
+ boost::shared_ptr<Msg> msg_ptr(new Msg);
+ msg_ptr->header.stamp = time;
+ msg_ptr->data = data;
+ return msg_ptr;
+}
+
+GateMsgConstPtr buildGateMsg(ros::Time time)
+{
+ boost::shared_ptr<GateMsg> msg_ptr(new GateMsg);
+ msg_ptr->header.stamp = time;
+ return msg_ptr;
+}
+
+class Helper
+{
+public:
+ Helper()
+ : count_(0)
+ {}
+
+ void cb(const SandwichElem<Msg,GateMsg>& sandwich_elem)
+ {
+ ++count_;
+ latest_elem_ = sandwich_elem;
+ }
+
+ int32_t count_;
+ SandwichElem<Msg,GateMsg> latest_elem_;
+};
+
+TEST(Sandwich, easyNoPadding)
+{
+ Cache<GateMsg> cache(10);
+ Sandwich<Msg, GateMsg> sandwich;
+ Helper h;
+ //cache.connect(boost::bind(&Sandwich<Msg,GateMsg>::addToGate, &sandwich, _1));
+ sandwich.connectCacheInput(cache);
+ sandwich.registerCallback(boost::bind(&Helper::cb, &h, _1));
+
+ cache.add(buildGateMsg(ros::Time(10,0)));
+ EXPECT_EQ(0, h.count_);
+ cache.add(buildGateMsg(ros::Time(20,0)));
+ EXPECT_EQ(0, h.count_);
+ sandwich.addToQueue(buildMsg(ros::Time(15,0),1));
+ EXPECT_EQ(1, h.count_);
+ EXPECT_EQ((unsigned int) 2, h.latest_elem_.interval.size());
+ EXPECT_EQ(h.latest_elem_.interval[0]->header.stamp, ros::Time(10,0));
+ EXPECT_EQ(h.latest_elem_.interval[1]->header.stamp, ros::Time(20,0));
+
+ sandwich.addToQueue(buildMsg(ros::Time(25,0),2));
+ EXPECT_EQ(1, h.count_);
+ cache.add(buildGateMsg(ros::Time(30,0)));
+ EXPECT_EQ(2, h.count_);
+ EXPECT_EQ((unsigned int) 2, h.latest_elem_.interval.size());
+ EXPECT_EQ(h.latest_elem_.interval[0]->header.stamp, ros::Time(20,0));
+ EXPECT_EQ(h.latest_elem_.interval[1]->header.stamp, ros::Time(30,0));
+
+ sandwich.addToQueue(buildMsg(ros::Time(5,0),2));
+ EXPECT_EQ(2, h.count_);
+}
+
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
Modified: pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-08-04 20:59:30 UTC (rev 20694)
@@ -5,13 +5,15 @@
genmsg()
-rospack_add_library(${PROJECT_NAME} src/stereo_cb_action.cpp)
+rospack_add_library(${PROJECT_NAME} #src/stereo_cb_action.cpp
+ src/capture_hand_led.cpp)
+
rospack_add_boost_directories()
rospack_link_boost(${PROJECT_NAME} thread)
-rospack_add_executable(run_action_stereo_cb src/run_action_stereo_cb.cpp)
-target_link_libraries(run_action_stereo_cb ${PROJECT_NAME})
+#rospack_add_executable(run_action_stereo_cb src/run_action_stereo_cb.cpp)
+#target_link_libraries(run_action_stereo_cb ${PROJECT_NAME})
rospack_add_executable(test_stereo_cb src/test_stereo_cb.cpp)
target_link_libraries(test_stereo_cb ${PROJECT_NAME})
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/capture_hand_led.h
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/capture_hand_led.h (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/capture_hand_led.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,98 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#ifndef PR2_CALIBRATION_ACTIONS_CAPTURE_HAND_LED_H_
+#define PR2_CALIBRATION_ACTIONS_CAPTURE_HAND_LED_H_
+
+#include "ros/ros.h"
+#include "message_filters/cache.h"
+#include "message_filters/subscriber.h"
+#include "message_filters/time_synchronizer.h"
+#include "calibration_msgs/ImagePointStamped.h"
+#include "sensor_msgs/Image.h"
+#include "calibration_message_filters/deflated.h"
+#include "calibration_message_filters/joint_states_deflater.h"
+#include "calibration_message_filters/stationary_checker.h"
+
+// msgs
+#include "mechanism_msgs/JointStates.h"
+
+namespace pr2_calibration_actions
+{
+
+class CaptureHandLED
+{
+public:
+ CaptureHandLED(ros::NodeHandle nh);
+
+
+
+private:
+
+ ros::NodeHandle nh_;
+
+ // ***** LED Image/Pixel Stuff *****
+ typedef calibration_message_filters::DeflatedMsg<sensor_msgs::Image> DeflatedImage;
+ typedef boost::shared_ptr<DeflatedImage> DeflatedImagePtr;
+ typedef boost::shared_ptr<const DeflatedImage> DeflatedImageConstPtr;
+
+ message_filters::Subscriber<calibration_msgs::ImagePointStamped> led_sub_;
+ message_filters::Subscriber<sensor_msgs::Image> led_image_sub_;
+ message_filters::TimeSynchronizer<calibration_msgs::ImagePointStamped, sensor_msgs::Image> sync_;
+ message_filters::Cache<DeflatedImage> led_cache_;
+
+ // ***** JointStates Stuff *****
+ typedef calibration_message_filters::DeflatedMsg<mechanism_msgs::JointStates> DeflatedJointStates;
+ typedef boost::shared_ptr<DeflatedJointStates> DeflatedJointStatesPtr;
+ typedef boost::shared_ptr<const DeflatedJointStates> DeflatedJointStatesConstPtr;
+
+ ros::Subscriber joint_states_sub_;
+ calibration_message_filters::JointStatesDeflater joint_states_deflater_;
+ message_filters::Cache<DeflatedJointStates> joint_states_cache_;
+
+ // ***** Stationary Check Stuff *****
+ calibration_message_filters::StationaryChecker stationary_checker_;
+
+ // ***** Implementation *****
+ void ledCallback(const calibration_msgs::ImagePointStampedConstPtr& led,
+ const sensor_msgs::ImageConstPtr& led_image);
+ void jointStatesCallback(const mechanism_msgs::JointStatesConstPtr& joint_states);
+ void checkStationary();
+};
+
+
+}
+
+
+#endif
Modified: pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml 2009-08-04 20:59:30 UTC (rev 20694)
@@ -14,7 +14,12 @@
<depend package="pr2_msgs" />
<depend package="robot_actions" />
-
+ <depend package="actionlib" />
+ <depend package="message_filters" />
+ <depend package="calibration_msgs" />
+ <depend package="mechanism_msgs" />
+ <depend package="calibration_message_filters" />
+
<export>
<cpp cflags="-I${prefix}/msg/cpp" />
</export>
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_hand_led.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_hand_led.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_hand_led.cpp 2009-08-04 20:59:30 UTC (rev 20694)
@@ -0,0 +1,126 @@
+/*********************************************************************
+* 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 "pr2_calibration_actions/capture_hand_led.h"
+
+
+using namespace pr2_calibration_actions;
+using namespace message_filters;
+using namespace std;
+using namespace calibration_message_filters;
+
+CaptureHandLED::CaptureHandLED(ros::NodeHandle nh) :
+ nh_(nh),
+ sync_(led_sub_, led_image_sub_, 1)
+{
+ // Configure the LED inputs
+ led_sub_.subscribe(nh_, "led", 1);
+ led_image_sub_.subscribe(nh_, "led_image", 1);
+ sync_.registerCallback( boost::bind(&CaptureHandLED::ledCallback, this, _1, _2) );
+ led_cache_.setCacheSize(100); //! \todo Don't hardcode cache size
+
+ // Configure the JointStates input
+ joint_states_sub_ = nh_.subscribe("joint_states", 1, &CaptureHandLED::jointStatesCallback, this);
+ joint_states_cache_.setCacheSize(100);
+
+ //! \todo This joint names should be params
+ vector<string> joint_names;
+ joint_names.push_back("r_shoulder_pan_joint");
+ joint_names.push_back("r_shoulder_lift_joint");
+ joint_names.push_back("r_upperarm_roll_joint");
+ joint_states_deflater_.setDeflationJointNames(joint_names);
+
+ // Configure the stationary checker
+ ChannelTolerance led_tol;
+ led_tol.tol_.push_back(1.01);
+ led_tol.tol_.push_back(1.01);
+
+ ChannelTolerance joint_states_tol;
+ joint_states_tol.tol_.push_back(0.01);
+ joint_states_tol.tol_.push_back(0.01);
+ joint_states_tol.tol_.push_back(0.01);
+
+ vector<Cache<Deflated>*> cache_vec(2);
+ cache_vec[0] = static_cast<Cache<Deflated>*>((void*) &led_cache_);
+ cache_vec[1] = static_cast<Cache<Deflated>*>((void*) &joint_states_cache_);
+
+ vector<ChannelTolerance> tolerance_vec(2);
+ tolerance_vec[0] = led_tol;
+ tolerance_vec[1] = joint_states_tol;
+
+ stationary_checker_.init(cache_vec, tolerance_vec);
+}
+
+
+void CaptureHandLED::ledCallback(const calibration_msgs::ImagePointStampedConstPtr& led,
+ const sensor_msgs::ImageConstPtr& led_image)
+{
+ DeflatedImagePtr deflated_ptr(new DeflatedImage);
+ deflated_ptr->header = led->header;
+ deflated_ptr->deflated_.resize(2);
+ deflated_ptr->deflated_[0] = led->image_point.x;
+ deflated_ptr->deflated_[1] = led->image_point.y;
+ deflated_ptr->msg = led_image;
+ ROS_DEBUG("Adding to LED Cache");
+ led_cache_.add(deflated_ptr);
+
+ checkStationary();
+}
+
+void CaptureHandLED::jointStatesCallback(const mechanism_msgs::JointStatesConstPtr& joint_states)
+{
+ DeflatedJointStatesPtr deflated_ptr(new DeflatedJointStates);
+ joint_states_deflater_.deflate(joint_states, *deflated_ptr);
+ ROS_DEBUG("Adding to JointStates Cache");
+ joint_states_cache_.add(deflated_ptr);
+
+ //checkStationary();
+}
+
+void CaptureHandLED::checkStationary()
+{
+ ros::Time stationary_time;
+ ros::Duration interval_duration(1,0);
+
+ stationary_time = stationary_checker_.isLatestStationary(interval_duration);
+
+ if (stationary_time != ros::Time(0,0))
+ ROS_INFO("Found a stationary elem");
+ else
+ ROS_DEBUG("Not stationary");
+}
+
+
+
+
Modified: pkg/trunk/vision/led_detection/include/led_detection/led_detector.h
===================================================================
--- pkg/trunk/vision/led_detection/include/led_detection/led_detector.h 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/vision/led_detection/include/led_detection/led_detector.h 2009-08-04 20:59:30 UTC (rev 20694)
@@ -42,8 +42,8 @@
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"
-#include "kinematic_calibration/ImagePoint.h"
#include "geometry_msgs/Pose.h"
+#include "calibration_msgs/ImagePoint.h"
namespace led_detection
{
@@ -67,7 +67,7 @@
*/
bool findLed(sensor_msgs::Image& image, const sensor_msgs::CameraInfo& info,
const geometry_msgs::Pose* led_pose,
- kinematic_calibration::ImagePoint& led_pix, sensor_msgs::Image& debug_image) ;
+ calibration_msgs::ImagePoint& led_pix, sensor_msgs::Image& debug_image) ;
/**
* \brief Does the 'heavy lifting' and openCV calls for finding the LED in an image.
@@ -82,7 +82,7 @@
*/
bool findLed(const IplImage* image, const sensor_msgs::CameraInfo& info,
const geometry_msgs::Pose* led_pose,
- kinematic_calibration::ImagePoint& led_pix, IplImage* debug_image) ;
+ calibration_msgs::ImagePoint& led_pix, IplImage* debug_image) ;
private:
Modified: pkg/trunk/vision/led_detection/manifest.xml
===================================================================
--- pkg/trunk/vision/led_detection/manifest.xml 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/vision/led_detection/manifest.xml 2009-08-04 20:59:30 UTC (rev 20694)
@@ -13,5 +13,5 @@
<depend package="robot_msgs" />
<depend package="topic_synchronizer" />
<depend package="kinematic_calibration" />
-
+ <depend package="calibration_msgs" />
</package>
Modified: pkg/trunk/vision/led_detection/src/led_detector.cpp
===================================================================
--- pkg/trunk/vision/led_detection/src/led_detector.cpp 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/vision/led_detection/src/led_detector.cpp 2009-08-04 20:59:30 UTC (rev 20694)
@@ -44,7 +44,8 @@
using namespace led_detection ;
using namespace sensor_msgs ;
-using namespace kinematic_calibration ;
+using namespace calibration_msgs ;
+using namespace geometry_msgs ;
// ******************** Helper Functions (Hack) ********************8
template <typename T>
@@ -169,7 +170,7 @@
bool LedDetector::findLed(const IplImage* image, const sensor_msgs::CameraInfo& info,
const geometry_msgs::Pose* led_pose,
- kinematic_calibration::ImagePoint& led_pix, IplImage* debug_image)
+ calibration_msgs::ImagePoint& led_pix, IplImage* debug_image)
{
IplImage* working = cvCloneImage(image) ;
int smooth_type = CV_GAUSSIAN ;
Modified: pkg/trunk/vision/led_detection/src/led_detector_node.cpp
===================================================================
--- pkg/trunk/vision/led_detection/src/led_detector_node.cpp 2009-08-04 20:56:29 UTC (rev 20693)
+++ pkg/trunk/vision/led_detection/src/led_detector_node.cpp 2009-08-04 20:59:30 UTC (rev 20694)
@@ -38,7 +38,7 @@
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PointStamped.h"
-#include "kinematic_calibration/ImagePointStamped.h"
+#include "calibration_msgs/ImagePointStamped.h"
using namespace led_detection ;
@@ -75,7 +75,7 @@
sync_.subscribe("image", image_msg_, 1) ;
sync_.subscribe("cam_info", cam_info_msg_, 1) ;
- node_->advertise<kinematic_calibration::ImagePointStamped>("~led", 10) ; //!todo Magic #
+ node_->advertise<calibration_msgs::ImagePointStamped>("~led", 10) ; //!todo Magic #
node_->advertise<sensor_msgs::Image>("~debug_image", 1) ;
sync_.ready() ;
@@ -97,7 +97,7 @@
led_world.pose.orientation.z = 0.0 ;
led_world.pose.orientation.w = 1.0 ;
- kinematic_calibration::ImagePointStamped led_pix ;
+ calibration_msgs::ImagePointStamped led_pix ;
bool found ;
if (use_led_pose_)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
