Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17925] pkg/trunk
|
From: <vij...@us...> - 2009-06-30 01:03:25
|
Revision: 17925
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17925&view=rev
Author: vijaypradeep
Date: 2009-06-30 00:35:56 +0000 (Tue, 30 Jun 2009)
Log Message:
-----------
Adding a message cache in the prototype message_filter design. Also still working on C++ dense laser assembler stuff. First cut of DenseLaserSnapshotter seems to work
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
pkg/trunk/sandbox/message_filters/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/calibration/calibration_msgs/
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/
pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/build_dense_laser_snapshot.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/tagged_laser_cache_display.cpp
pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h
pkg/trunk/sandbox/message_filters/src/filter_example.cpp
pkg/trunk/sandbox/message_filters/test/
pkg/trunk/sandbox/message_filters/test/CMakeLists.txt
pkg/trunk/sandbox/message_filters/test/msg_cache_unittest.cpp
Removed Paths:
-------------
pkg/trunk/sandbox/message_filters/src/sync_test.cpp
Added: pkg/trunk/calibration/calibration_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/calibration_msgs/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/CMakeLists.txt 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+rospack(calibration_msgs)
+
+genmsg()
+
Added: pkg/trunk/calibration/calibration_msgs/Makefile
===================================================================
--- pkg/trunk/calibration/calibration_msgs/Makefile (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/Makefile 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/calibration/calibration_msgs/manifest.xml
===================================================================
--- pkg/trunk/calibration/calibration_msgs/manifest.xml (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/manifest.xml 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,20 @@
+<package>
+ <description brief="calibration_msgs">
+ Messages for collecting datasets to perform various robot calibrations.
+ </description>
+ <author>Vijay Pradeep / vpr...@wi...</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/calibration_msgs</url>
+
+ <depend package="std_msgs" />
+ <depend package="robot_msgs" />
+ <depend package="image_msgs" />
+
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp" />
+ </export>
+
+</package>
+
+
Added: pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,38 @@
+# Provides all the state & sensor information for
+# a single sweep of laser attached to some mechanism.
+# Most likely, this 'mechanism' will be the tilting stage
+Header header
+
+# Store the laser intrinsics. This is very similar to the
+# intrinsics commonly stored in
+float32 angle_min # start angle of the scan [rad]
+float32 angle_max # end angle of the scan [rad]
+float32 angle_increment # angular distance between measurements [rad]
+float32 time_increment # time between measurements [seconds]
+float32 range_min # minimum range value [m]
+float32 range_max # maximum range value [m]
+
+# Define the size of the binary data
+uint32 readings_per_scan # (Width)
+uint32 num_scans # (Height)
+
+# 2D Arrays storing laser data.
+# We can think of each type data as being a single channel image.
+# Each row of the image has data from a single scan, and scans are
+# concatenated to form the entire 'image'.
+float32[] ranges # (Image data)
+float32[] intensities # (Image data)
+
+# Joint information
+# Store joint information associated with each scan
+uint32 joint_encoding
+float64[] joint_positions
+string[] joint_names
+uint32 POS_PER_PIXEL = 0
+uint32 POS_PER_ROW_START = 1
+uint32 POS_PER_ROW_START_AND_END = 2
+
+# 1D Data storing meta information for each scan/row
+# Currently in stable, time vectors are not serialized correctly
+#time[] scan_start
+
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt 2009-06-30 00:07:03 UTC (rev 17924)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt 2009-06-30 00:35:56 UTC (rev 17925)
@@ -8,23 +8,24 @@
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE Debug)
rospack(dense_laser_assembler)
-#set the default path for built executables to the "bin" directory
+rospack_add_library(dense_laser_assembler src/build_dense_laser_snapshot.cpp)
+rospack_add_executable(dense_laser_assembler_srv src/dense_laser_assembler_srv.cpp)
+target_link_libraries(dense_laser_assembler_srv dense_laser_assembler)
+
+#rospack_add_executable(joint_extractor_display src/joint_extractor_display.cpp)
+#rospack_add_executable(tagged_laser_cache_display src/tagged_laser_cache_display.cpp)
+
+rospack_add_executable(dense_laser_snapshotter src/dense_laser_snapshotter.cpp)
+
+#rospack_add_executable(talker src/talker.cpp)
+#rospack_add_executable(listener src/listener.cpp)
+
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
genmsg()
-#uncomment if you have defined services
-#gensrv()
-
-#common commands for building c++ executables and libraries
-#rospack_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rospack_add_boost_directories()
-#rospack_link_boost(${PROJECT_NAME} thread)
-#rospack_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
+gensrv()
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-06-30 00:07:03 UTC (rev 17924)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-06-30 00:35:56 UTC (rev 17925)
@@ -17,9 +17,12 @@
<depend package="laser_scan"/>
<depend package="pr2_mechanism_controllers"/>
<depend package="image_msgs" />
-
+ <depend package="mechanism_msgs" />
+ <depend package="message_filters" />
+ <depend package="calibration_msgs" />
+
<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp -I{prefix}/include" />
</export>
</package>
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/build_dense_laser_snapshot.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/build_dense_laser_snapshot.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/build_dense_laser_snapshot.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,160 @@
+/*********************************************************************
+* 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 "dense_laser_assembler/build_dense_laser_snapshot.h"
+
+using namespace std ;
+using namespace boost ;
+
+namespace dense_laser_assembler
+{
+
+bool verifyMetadata(const calibration_msgs::DenseLaserSnapshot& snapshot, const laser_scan::LaserScan& scan) ;
+
+bool buildDenseLaserSnapshot(const vector<shared_ptr<const JointTaggedLaserScan> >& scans,
+ const vector<string>& joint_names,
+ calibration_msgs::DenseLaserSnapshot& snapshot)
+{
+ if (scans.size() == 0)
+ {
+ ROS_WARN("Trying to build empty cloud") ;
+ snapshot.angle_min = 0.0 ;
+ snapshot.angle_max = 0.0 ;
+ snapshot.angle_increment = 0.0 ;
+ snapshot.time_increment = 0.0 ;
+ snapshot.range_min = 0.0 ;
+ snapshot.range_max = 0.0 ;
+ snapshot.readings_per_scan = 0 ;
+ snapshot.num_scans = 0 ;
+ snapshot.ranges.clear() ;
+ snapshot.intensities.clear() ;
+ snapshot.joint_encoding = 0 ;
+ snapshot.joint_names.clear() ;
+ snapshot.joint_positions.clear() ;
+ //snapshot.scan_start.clear() ;
+ return true ;
+ }
+
+ snapshot.header.stamp = scans[scans.size()-1]->scan->header.stamp ;
+
+ // Fill in all the metadata
+ snapshot.header.frame_id = scans[0]->scan->header.frame_id ;
+ snapshot.angle_min = scans[0]->scan->angle_min ;
+ snapshot.angle_max = scans[0]->scan->angle_max ;
+ snapshot.angle_increment = scans[0]->scan->angle_increment ;
+ snapshot.time_increment = scans[0]->scan->time_increment ;
+ snapshot.range_min = scans[0]->scan->range_min ;
+ snapshot.range_max = scans[0]->scan->range_max ;
+
+ // Define the data dimensions
+ snapshot.readings_per_scan = scans[0]->scan->ranges.size() ;
+ snapshot.num_scans = scans.size() ;
+ const unsigned int& w = snapshot.readings_per_scan ;
+ const unsigned int& h = snapshot.num_scans ;
+
+ // Do a consistency check on the metadata
+ for (unsigned int i=0; i<scans.size(); i++)
+ {
+ if (!verifyMetadata(snapshot, *scans[i]->scan))
+ {
+ ROS_WARN("Metadata doesn't match") ;
+ return false ;
+ }
+ }
+
+ // Set up joint data vectors
+ const unsigned int num_joints = joint_names.size() ;
+ snapshot.joint_names = joint_names ;
+ snapshot.joint_encoding = calibration_msgs::DenseLaserSnapshot::POS_PER_ROW_START_AND_END ;
+ snapshot.joint_positions.resize(num_joints*h*2) ; // 2 Positions per joint per row (beginning and end of row)
+
+ // Set up other data vectors
+ //snapshot.scan_start.resize(h) ;
+ snapshot.ranges.resize(w*h) ;
+ snapshot.intensities.resize(w*h) ;
+
+ const unsigned int range_elem_size = sizeof(scans[0]->scan->ranges[0]) ;
+ const unsigned int intensity_elem_size = sizeof(scans[0]->scan->intensities[0]) ;
+
+ for (unsigned int i=0; i<h; i++)
+ {
+ memcpy(&snapshot.ranges[i*w], &scans[i]->scan->ranges[0], w*range_elem_size) ;
+ memcpy(&snapshot.intensities[i*w], &scans[i]->scan->intensities[0], w*intensity_elem_size) ;
+
+ // Copy joint positions
+ for (unsigned int j=0; j<num_joints; j++)
+ {
+ snapshot.joint_positions[i*j*2 + 0] = scans[i]->before->positions[j] ;
+ snapshot.joint_positions[i*j*2 + 1] = scans[i]->after->positions[j] ;
+ }
+
+ // Copy time stamp
+ //snapshot.scan_start[i] = scans[i]->header.stamp ;
+ }
+
+ ROS_INFO("Done building snapshot that is [%u rows] x [%u cols]\n", h, w) ;
+
+ return true ;
+}
+
+static const double eps = 1e-9 ;
+
+#define CHECK(a) \
+{ \
+ if ( (snapshot.a - scan.a < -eps) || (snapshot.a - scan.a > eps)) \
+ return false ; \
+}
+
+bool verifyMetadata(const calibration_msgs::DenseLaserSnapshot& snapshot, const laser_scan::LaserScan& scan)
+{
+ CHECK(angle_min) ;
+ CHECK(angle_max) ;
+ CHECK(angle_increment) ;
+ CHECK(time_increment) ;
+ CHECK(range_min) ;
+ CHECK(range_max) ;
+
+ if (snapshot.header.frame_id.compare(scan.header.frame_id) != 0)
+ return false ;
+
+ if (snapshot.readings_per_scan != scan.ranges.size())
+ return false ;
+ if (snapshot.readings_per_scan != scan.intensities.size())
+ return false ;
+
+
+ return true ;
+}
+
+}
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler_srv.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler_srv.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler_srv.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,161 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+//! \author Vijay Pradeep / vpr...@wi...
+
+#include "ros/ros.h"
+#include "dense_laser_assembler/build_dense_laser_snapshot.h"
+#include "dense_laser_assembler/joint_extractor.h"
+#include "dense_laser_assembler/laser_scan_tagger.h"
+#include "message_filters/msg_cache.h"
+
+//#include "dense_laser_assembler/test_func.h"
+
+// Messages/Services
+#include "dense_laser_assembler/BuildLaserSnapshot.h"
+
+#define CONSTRUCT_INT(param, default_val) \
+ int param ;\
+ if (!n_.getParam("~" #param, param) ) \
+ { \
+ ROS_WARN("[~" #param "] not set. Setting to default value of [%u]", default_val) ; \
+ param = default_val ; \
+ } \
+ else \
+ { \
+ ROS_INFO("[~" #param "] set to value of [%u]", param) ; \
+ }
+
+using namespace dense_laser_assembler ;
+using namespace std ;
+
+class DenseLaserAssemblerSrv
+{
+public:
+ DenseLaserAssemblerSrv()
+ {
+ CONSTRUCT_INT(laser_queue_size, 40) ;
+ CONSTRUCT_INT(laser_cache_size, 1000) ;
+ CONSTRUCT_INT(mech_state_cache_size, 100) ;
+
+ bool found_joint = true ;
+ int joint_count = 0 ;
+
+ char param_buf[1024] ;
+ while(found_joint)
+ {
+ sprintf(param_buf, "~joint_name_%02u", joint_count) ;
+ string param_name = param_buf ;
+ string cur_joint_name ;
+ found_joint = n_.getParam(param_name, cur_joint_name) ;
+ if (found_joint)
+ {
+ ROS_INFO("[%s] -> %s\n", param_name.c_str(), cur_joint_name.c_str()) ;
+ joint_names_.push_back(cur_joint_name) ;
+ }
+ }
+
+
+ // Configure the joint extractor
+ joint_extractor_.setJointNames(joint_names_) ;
+ joint_cache_.setCacheSize(mech_state_cache_size) ;
+ joint_cache_.subscribe(joint_extractor_) ;
+
+ // Set up the laser tagger (and link it to the joint_cache)
+ laser_tagger_.setTagCache(joint_cache_) ;
+ laser_tagger_.setQueueSize(laser_queue_size) ;
+
+ // Trigger laser_tagger off updates to the joint_cache
+ joint_cache_.addOutputCallback(boost::bind(&LaserJointTagger::update, &laser_tagger_)) ;
+
+ // Set up the cache for tagged laser scans (and link it to the laser_tagger)
+ tagged_laser_cache_.setCacheSize(laser_cache_size) ;
+ tagged_laser_cache_.subscribe(laser_tagger_) ;
+
+ // Link the joint extractor to a topic
+ mech_sub_ = n_.subscribe("mechanism_state", 1, &JointExtractor::processMechState, &joint_extractor_) ;
+
+ // Link the laser_tagger to a topic
+ laser_sub_ = n_.subscribe("tilt_scan", 1, &LaserJointTagger::processLaserScan, &laser_tagger_) ;
+
+ // Advertise services
+ snapshot_adv_ = n_.advertiseService("~build_laser_snapshot", &DenseLaserAssemblerSrv::buildLaserSnapshotSrv, this) ;
+ }
+
+ bool buildLaserSnapshotSrv(BuildLaserSnapshot::Request& req, BuildLaserSnapshot::Response& resp)
+ {
+ vector<boost::shared_ptr<const JointTaggedLaserScan> > scans = tagged_laser_cache_.getInterval(req.start, req.end) ;
+ bool success = true ;
+ success = buildDenseLaserSnapshot(scans, joint_names_, resp.snapshot) ;
+
+ /*printf("Displaying times:\n") ;
+ for (unsigned int i=0; i<resp.snapshot.scan_start.size(); i++)
+ {
+ printf(" %u: %f\n", i, resp.snapshot.scan_start[i].toSec()) ;
+ }*/
+
+ return success ;
+ }
+
+private:
+ ros::NodeHandle n_ ;
+ ros::Subscriber mech_sub_ ;
+ ros::Subscriber laser_sub_ ;
+ ros::ServiceServer snapshot_adv_ ;
+
+ //! Extracts positions data for a subset of joints in MechanismState
+ JointExtractor joint_extractor_ ;
+ //! Stores a time history of position data for a subset of joints in MechanismState
+ message_filters::MsgCache<JointExtractor::JointArray> joint_cache_ ;
+
+ //! Combines a laser scan with the set of pertinent joint positions
+ LaserJointTagger laser_tagger_ ;
+
+ //! Stores a time history of laser scans that are annotated with joint positions.
+ JointTaggedLaserScanCache tagged_laser_cache_ ;
+
+ vector<string> joint_names_ ;
+} ;
+
+
+
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "dense_laser_assembler_srv") ;
+
+ DenseLaserAssemblerSrv assembler ;
+
+ ros::spin() ;
+}
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,132 @@
+/*********************************************************************
+* 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 "ros/ros.h"
+
+// Services
+#include "dense_laser_assembler/BuildLaserSnapshot.h"
+
+// Messages
+#include "calibration_msgs/DenseLaserSnapshot.h"
+#include "pr2_mechanism_controllers/LaserScannerSignal.h"
+
+
+using namespace dense_laser_assembler ;
+
+class DenseLaserSnapshotter
+{
+
+public:
+
+ DenseLaserSnapshotter()
+ {
+ prev_signal_.header.stamp.fromNSec(0) ;
+
+ snapshot_pub_ = n_.advertise<calibration_msgs::DenseLaserSnapshot> ("dense_laser_snapshot", 1) ;
+ signal_sub_ = n_.subscribe("laser_scanner_signal", 40, &DenseLaserSnapshotter::scannerSignalCallback, this) ;
+
+ first_time_ = true ;
+ }
+
+ ~DenseLaserSnapshotter()
+ {
+
+ }
+
+ void scannerSignalCallback(const pr2_mechanism_controllers::LaserScannerSignalConstPtr& cur_signal)
+ {
+ if (cur_signal->signal == 128 || cur_signal->signal == 129) // These codes imply that this is the first signal during a given profile type
+ first_time_ = true ;
+
+
+ if (first_time_)
+ {
+ prev_signal_ = *cur_signal ;
+ first_time_ = false ;
+ }
+ else
+ {
+ printf("About to make request\n") ;
+
+ BuildLaserSnapshot::Request req ;
+ BuildLaserSnapshot::Response resp ;
+
+ req.start = prev_signal_.header.stamp ;
+ req.end = cur_signal->header.stamp ;
+
+
+
+ if (!ros::service::call("dense_laser_assembler_srv/build_laser_snapshot", req, resp))
+ ROS_ERROR("Failed to call service on dense laser assembler.");
+
+ printf("Displaying Data") ;
+ printf("header.stamp: %f\n", resp.snapshot.header.stamp.toSec()) ;
+ printf("header.frame_id: %s", resp.snapshot.header.frame_id.c_str()) ;
+ printf("ranges.size()=%u\n", resp.snapshot.ranges.size()) ;
+ printf("intensities.size()=%u\n", resp.snapshot.intensities.size()) ;
+ printf("joint_positions.size()=%u\n", resp.snapshot.joint_positions.size()) ;
+ //printf("scan_start.size()=%u\n", resp.snapshot.scan_start.size()) ;
+
+ /*printf("Displaying times:\n") ;
+ for (unsigned int i=0; i<resp.snapshot.scan_start.size(); i++)
+ {
+ printf(" %u: %f\n", i, resp.snapshot.scan_start[i].toSec()) ;
+ }*/
+
+ /*printf("About to publish\n") ;
+ snapshot_pub_.publish(resp.snapshot) ;
+ printf("Done publishing\n") ;*/
+
+ prev_signal_ = *cur_signal ;
+ }
+ }
+
+private:
+ ros::NodeHandle n_ ;
+ ros::Subscriber signal_sub_ ;
+ ros::Publisher snapshot_pub_ ;
+
+ pr2_mechanism_controllers::LaserScannerSignal prev_signal_;
+
+ bool first_time_ ;
+} ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "dense_laser_snapshotter") ;
+ DenseLaserSnapshotter snapshotter ;
+ ros::spin() ;
+
+ return 0;
+}
Copied: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp (from rev 17924, pkg/trunk/sandbox/message_filters/src/sync_test.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,78 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/*! \mainpage
+ * \htmlinclude manifest.html
+ */
+
+#include "ros/ros.h"
+#include "dense_laser_assembler/joint_extractor.h"
+
+using namespace std ;
+//using namespace message_filters ;
+using namespace dense_laser_assembler ;
+
+
+
+void display_joints(vector<string> joint_names, const boost::shared_ptr<JointExtractor::JointArray const>& joint_array_ptr)
+{
+ printf("Joints:\n") ;
+ for (unsigned int i=0; i< joint_names.size(); i++)
+ {
+ printf(" %s: %f\n", joint_names[i].c_str(), joint_array_ptr->positions[i]) ;
+ }
+
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "joint_extractor_display") ;
+
+ ros::NodeHandle nh ;
+
+ vector<string> joint_names ;
+ joint_names.push_back("laser_tilt_mount_joint") ;
+ joint_names.push_back("torso_lift_link") ;
+
+ JointExtractor joint_extractor(joint_names) ;
+
+ joint_extractor.addOutputCallback(boost::bind(&display_joints, joint_names, _1) ) ;
+
+ ros::Subscriber sub = nh.subscribe("mechanism_state", 1, &JointExtractor::processMechState, &joint_extractor) ;
+
+ ros::spin() ;
+
+
+ return 0 ;
+}
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/tagged_laser_cache_display.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/tagged_laser_cache_display.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/tagged_laser_cache_display.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,106 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/*! \mainpage
+ * \htmlinclude manifest.html
+ */
+
+#include <string>
+#include <vector>
+
+#include "ros/ros.h"
+
+#include "message_filters/msg_cache.h"
+#include "dense_laser_assembler/joint_extractor.h"
+#include "dense_laser_assembler/laser_scan_tagger.h"
+
+using namespace std ;
+using namespace dense_laser_assembler ;
+using namespace message_filters ;
+
+typedef LaserScanTagger<JointExtractor::JointArray> LaserTagger ;
+
+void display_tagged_scan(const boost::shared_ptr<const LaserTagger::TaggedLaserScan>& tagged_scan )
+{
+ printf("TaggedScan:\n") ;
+ printf(" Laser Scan Time: %f\n", tagged_scan->scan->header.stamp.toSec()) ;
+ printf(" JointArray before: %f\n", tagged_scan->before->header.stamp.toSec()) ;
+ printf(" JointArray after: %f\n", tagged_scan->after->header.stamp.toSec()) ;
+ printf(" Joints:\n") ;
+
+ for (unsigned int i=0; i< tagged_scan->before->positions.size(); i++)
+ {
+ printf(" %02u) %f->%f\n", i, tagged_scan->before->positions[i], tagged_scan->after->positions[i]) ;
+ }
+ printf("*****\n") ;
+
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "tagged_laser_cache_display") ;
+
+ ros::NodeHandle nh ;
+
+ // Define which links we want to listen to
+ vector<string> joint_names ;
+ joint_names.push_back("laser_tilt_mount_joint") ;
+ joint_names.push_back("torso_lift_link") ;
+
+ // Set up the joint extraction and cache
+ JointExtractor joint_extractor(joint_names) ;
+ MsgCache<JointExtractor::JointArray> joint_cache(100) ;
+ joint_cache.subscribe(joint_extractor) ;
+
+ // Set up the laser tagger (and link it to the joint_cache)
+ LaserTagger laser_tagger(joint_cache, 40) ;
+ joint_cache.addOutputCallback(boost::bind(&LaserTagger::update, &laser_tagger)) ;
+ laser_tagger.addOutputCallback(&display_tagged_scan) ;
+
+ // Set up the cache for tagged laser scans (and link it to the laser_tagger)
+ MsgCache<LaserTagger::TaggedLaserScan> tagged_laser_cache(40) ;
+ tagged_laser_cache.subscribe(laser_tagger) ;
+
+
+ // Link the joint extractor to a topic
+ ros::Subscriber mech_sub = nh.subscribe("mechanism_state", 1, &JointExtractor::processMechState, &joint_extractor) ;
+
+ // Link the laser_tagger to a topic
+ ros::Subscriber laser_sub = nh.subscribe("tilt_scan", 1, &LaserTagger::processLaserScan, &laser_tagger) ;
+
+ ros::spin() ;
+
+
+ return 0 ;
+}
Modified: pkg/trunk/sandbox/message_filters/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/message_filters/CMakeLists.txt 2009-06-30 00:07:03 UTC (rev 17924)
+++ pkg/trunk/sandbox/message_filters/CMakeLists.txt 2009-06-30 00:35:56 UTC (rev 17925)
@@ -2,6 +2,9 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(message_filters)
-rospack_add_executable(sync_test src/sync_test.cpp)
+rospack_add_executable(filter_example src/filter_example.cpp)
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
\ No newline at end of file
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+add_subdirectory(test EXCLUDE_FROM_ALL)
Added: pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h (rev 0)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,247 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/*! \mainpage
+ * \htmlinclude manifest.html
+ */
+
+#ifndef MESSAGE_FILTERS_MSG_CACHE_H_
+#define MESSAGE_FILTERS_MSG_CACHE_H_
+
+#include <deque>
+#include "boost/thread.hpp"
+#include "boost/shared_ptr.hpp"
+#include "ros/time.h"
+
+namespace message_filters
+{
+
+/**
+ * \brief Stores a time history of messages
+ * Given a stream of messages, the most recent N messages are cached in a ring buffer,
+ * from which time intervals of the cache can then be retrieved by the client. This assumes
+ * that the messages are being received with monotonically increasing timestamps in header.stamp.
+ */
+template<class M>
+class MsgCache
+{
+public:
+ typedef boost::shared_ptr<M const> MsgPtr ;
+
+ MsgCache(unsigned int cache_size = 1)
+ {
+ setCacheSize(cache_size) ;
+ }
+
+ /**
+ * Set the size of the cache.
+ * \param cache_size The new size the cache should be. Must be > 0
+ */
+ void setCacheSize(unsigned int cache_size)
+ {
+ if (cache_size == 0)
+ {
+ //ROS_ERROR("Cannot set max_size to 0") ;
+ return ;
+ }
+
+ cache_size_ = cache_size ;
+ }
+
+
+ template<class T>
+ /**
+ * Links Consumer's input to some provider's output.
+ * \param provider The filter from which we want to receive data
+ */
+ void subscribe(T& provider)
+ {
+ //printf("Called MsgCache Subscribe\n") ;
+ provider.addOutputCallback(boost::bind(&MsgCache::addToCache, this, _1)) ;
+ }
+
+ /**
+ * Add the most recent message to the cache, and pop off any elements that are too old.
+ * This method is registered with a data provider when subscribe is called.
+ */
+ void addToCache(const MsgPtr& msg)
+ {
+ //printf(" Cache Size: %u\n", cache_.size()) ;
+ cache_lock_.lock() ;
+
+ while (cache_.size() >= cache_size_) // Keep popping off old data until we have space for a new msg
+ cache_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
+
+ cache_.push_back(msg) ; // Add the newest message to the back of the deque
+ cache_lock_.unlock() ;
+
+ // Sequentially call each registered call
+ for (unsigned int i=0; i<output_callbacks_.size(); i++)
+ output_callbacks_[i]() ;
+ }
+
+ /**
+ * Called by another filter that wants the output of this filter
+ * \param callback The function that is called when data is available
+ */
+ void addOutputCallback(const boost::function<void()>& callback)
+ {
+ output_callbacks_.push_back(callback) ;
+ }
+
+
+ /**
+ * Receive a vector of messages that occur between a start and end time (inclusive).
+ * This call is non-blocking, and only aggregates messages it has already received.
+ * It will not wait for messages have not yet been received, but occur in the interval.
+ * \param start The start of the requested interval
+ * \param end The end of the requested interval
+ */
+ std::vector<MsgPtr> getInterval(const ros::Time& start, const ros::Time& end)
+ {
+ cache_lock_.lock() ;
+
+ // Find the starting index. (Find the first index after [or at] the start of the interval)
+ unsigned int start_index = 0 ;
+ while(start_index < cache_.size() &&
+ cache_[start_index]->header.stamp < 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 < cache_.size() &&
+ cache_[end_index]->header.stamp <= end)
+ {
+ end_index++ ;
+ }
+
+ std::vector<MsgPtr> interval_elems ;
+ interval_elems.reserve(end_index - start_index) ;
+ for (unsigned int i=start_index; i<end_index; i++)
+ {
+ interval_elems.push_back(cache_[i]) ;
+ }
+
+ cache_lock_.unlock() ;
+
+ return interval_elems ;
+ }
+
+ /**
+ * Grab the newest element that occurs right before the specified time.
+ * \param time Time that must occur right after the returned elem
+ * \returns shared_ptr to the newest elem that occurs before 'time'. NULL if doesn't exist
+ */
+ MsgPtr getElemBeforeTime(const ros::Time& time)
+ {
+ MsgPtr out ;
+
+ cache_lock_.lock() ;
+ unsigned int i=0 ;
+ int elem_index = -1 ;
+ while (i<cache_.size() &&
+ cache_[i]->header.stamp < time)
+ {
+ elem_index = i ;
+ i++ ;
+ }
+
+ if (elem_index >= 0)
+ out = cache_[elem_index] ;
+
+ cache_lock_.unlock() ;
+
+ return out ;
+ }
+
+ /**
+ * Grab the oldest element that occurs right after the specified time.
+ * \param time Time that must occur right before the returned elem
+ * \returns shared_ptr to the oldest elem that occurs after 'time'. NULL if doesn't exist
+ */
+ MsgPtr getElemAfterTime(const ros::Time& time)
+ {
+ MsgPtr out ;
+
+ cache_lock_.lock() ;
+
+ int i=cache_.size()-1 ;
+ int elem_index = -1 ;
+ while (i>=0 &&
+ cache_[i]->header.stamp > time)
+ {
+ elem_index = i ;
+ i-- ;
+ }
+
+ if (elem_index >= 0)
+ out = cache_[elem_index] ;
+ else
+ out.reset() ;
+
+ cache_lock_.unlock() ;
+
+ return out ;
+ }
+
+ /**
+ * Returns the timestamp associated with the newest packet cache
+ */
+ ros::Time getLatestTime()
+ {
+ ros::Time latest_time(0, 0) ;
+
+ cache_lock_.lock() ;
+ if (cache_.size() > 0)
+ latest_time = cache_.back()->header.stamp ;
+ cache_lock_.unlock() ;
+
+ return latest_time ;
+ }
+
+private:
+ boost::mutex cache_lock_ ; //!< Lock for cache_
+ std::deque<MsgPtr > cache_ ; //!< Cache for the messages
+ unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache.
+
+ //! Array of callbacks to be called whenever new data is received
+ std::vector<boost::function<void()> > output_callbacks_ ;
+} ;
+
+}
+
+
+#endif /* MSG_CACHE_H_ */
Copied: pkg/trunk/sandbox/message_filters/src/filter_example.cpp (from rev 17924, pkg/trunk/sandbox/message_filters/src/sync_test.cpp)
===================================================================
--- pkg/trunk/sandbox/message_filters/src/filter_example.cpp (rev 0)
+++ pkg/trunk/sandbox/message_filters/src/filter_example.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,69 @@
+/*********************************************************************
+* 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 "ros/ros.h"
+#include "robot_msgs/PointStamped.h"
+#include "robot_msgs/PointCloud.h"
+#include "image_msgs/CamInfo.h"
+#include "message_filters/topic_synchronizer_filter.h"
+#include "message_filters/sync_helper.h"
+#include "message_filters/consumer.h"
+#include "message_filters/msg_cache.h"
+
+using namespace std ;
+using namespace message_filters ;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "sync_test") ;
+
+ ros::NodeHandle nh ;
+
+ //TopicSynchronizerFilter<robot_msgs::PointStamped, robot_msgs::PointCloud> filter ;
+
+ // Define the source 'node'
+ SyncHelper<image_msgs::CamInfo> sync_helper("stereo/left/cam_info/", 10, nh) ;
+
+ Consumer consumer ;
+ // Link the consumer to the output of sync_helper
+ consumer.subscribe(sync_helper) ;
+
+ MsgCache<image_msgs::CamInfo> cache(10) ;
+ cache.subscribe(sync_helper) ;
+
+ ros::spin() ;
+
+
+ return 0 ;
+}
Deleted: pkg/trunk/sandbox/message_filters/src/sync_test.cpp
===================================================================
--- pkg/trunk/sandbox/message_filters/src/sync_test.cpp 2009-06-30 00:07:03 UTC (rev 17924)
+++ pkg/trunk/sandbox/message_filters/src/sync_test.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -1,66 +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 "ros/ros.h"
-#include "robot_msgs/PointStamped.h"
-#include "robot_msgs/PointCloud.h"
-#include "image_msgs/CamInfo.h"
-#include "message_filters/topic_synchronizer_filter.h"
-#include "message_filters/sync_helper.h"
-#include "message_filters/consumer.h"
-
-using namespace std ;
-using namespace message_filters ;
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "sync_test") ;
-
- ros::NodeHandle nh ;
-
- //TopicSynchronizerFilter<robot_msgs::PointStamped, robot_msgs::PointCloud> filter ;
-
- // Define the source 'node'
- SyncHelper<image_msgs::CamInfo> sync_helper("stereo/left/cam_info/", 10, nh) ;
-
- Consumer consumer ;
-
- // Link the consumer to the output of sync_helper
- consumer.subscribe(sync_helper) ;
-
- ros::spin() ;
-
-
- return 0 ;
-}
Added: pkg/trunk/sandbox/message_filters/test/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/message_filters/test/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/message_filters/test/CMakeLists.txt 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,4 @@
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
+
+# ********** Tests **********
+rospack_add_gtest(test/msg_cache_unittest msg_cache_unittest.cpp)
Added: pkg/trunk/sandbox/message_filters/test/msg_cache_unittest.cpp
===================================================================
--- pkg/trunk/sandbox/message_filters/test/msg_cache_unittest.cpp (rev 0)
+++ pkg/trunk/sandbox/message_filters/test/msg_cache_unittest.cpp 2009-06-30 00:35:56 UTC (rev 17925)
@@ -0,0 +1,119 @@
+/*********************************************************************
+* 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/msg_cache.h"
+
+using namespace std ;
+using namespace message_filters ;
+
+struct Header
+{
+ ros::Time stamp ;
+} ;
+
+
+struct Msg
+{
+ Header header ;
+ int data ;
+} ;
+
+
+
+
+void fill_cache_easy(MsgCache<Msg>& cache, unsigned int start, unsigned int end)
+{
+ for (unsigned int i=start; i < end; i++)
+ {
+ Msg* msg = new Msg ;
+ msg->data = i ;
+ msg->header.stamp.fromSec(i*10) ;
+
+ boost::shared_ptr<Msg const> msg_ptr(msg) ;
+ cache.addToCache(msg_ptr) ;
+ }
+}
+
+TEST(MESSAGE_FILTERS_MSG_CACHE, easy_interval_test)
+{
+ MsgCache<Msg> cache(10) ;
+ fill_cache_easy(cache, 0, 5) ;
+
+ vector<boost::shared_ptr<Msg const> > interval_data = cache.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 = cache.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
+ fill_cache_easy(cache, 5, 20) ;
+ interval_data = cache.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
+ EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
+}
+
+TEST(MESSAGE_FILTERS_MSG_CACHE, easy_elem_before_after)
+{
+ MsgCache<Msg> cache(10) ;
+ boost::shared_ptr<Msg const> elem_ptr ;
+
+ fill_cache_easy(cache, 5, 10) ;
+
+ elem_ptr = cache.getElemAfterTime( ros::Time().fromSec(85.0)) ;
+
+ ASSERT_FALSE(!elem_ptr) ;
+ EXPECT_EQ(elem_ptr->data, 9) ;
+
+ elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(85.0)) ;
+ ASSERT_FALSE(!elem_ptr) ;
+ EXPECT_EQ(elem_ptr->data, 8) ;
+
+ elem_ptr = cache.getElemBeforeTime( ros::Time().fromSec(45.0)) ;
+ EXPECT_TRUE(!elem_ptr) ;
+}
+
+
+
+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.
|
