Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18313] pkg/trunk
|
From: <vij...@us...> - 2009-07-06 17:22:56
|
Revision: 18313
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18313&view=rev
Author: vijaypradeep
Date: 2009-07-06 17:22:54 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
Renaming msg_cache to cache
Now the JointPVFilter outputs position and velocity info
Starting to work on getting the DenseLaserAssemblerFilter going
DenseLaserSnapshotter and DenseLaserAssemblerFilter seem to be working
Modified Paths:
--------------
pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
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_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
pkg/trunk/sandbox/message_filters/src/filter_example.cpp
pkg/trunk/sandbox/message_filters/test/msg_cache_unittest.cpp
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/msg/JointPVArray.msg
pkg/trunk/sandbox/message_filters/include/message_filters/cache.h
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h
pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h
Modified: pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/calibration/calibration_msgs/msg/DenseLaserSnapshot.msg 2009-07-06 17:22:54 UTC (rev 18313)
@@ -27,6 +27,7 @@
# Store joint information associated with each scan
uint32 joint_encoding
float64[] joint_positions
+float64[] joint_velocities
string[] joint_names
uint32 POS_PER_PIXEL = 0
uint32 POS_PER_ROW_START = 1
@@ -34,5 +35,5 @@
# 1D Data storing meta information for each scan/row
# Currently in stable, time vectors are not serialized correctly
-#time[] scan_start
+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-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/CMakeLists.txt 2009-07-06 17:22:54 UTC (rev 18313)
@@ -12,14 +12,16 @@
rospack(dense_laser_assembler)
-rospack_add_library(dense_laser_assembler src/build_dense_laser_snapshot.cpp)
+rospack_add_library(dense_laser_assembler src/build_dense_laser_snapshot.cpp
+ src/dense_laser_msg_filter.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(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)
+target_link_libraries(dense_laser_snapshotter dense_laser_assembler)
rospack_add_executable(laser_imager src/laser_imager.cpp)
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -42,16 +42,19 @@
#include <vector>
#include "dense_laser_assembler/laser_scan_tagger.h"
-#include "message_filters/msg_cache.h"
+#include "message_filters/cache.h"
+#include "dense_laser_assembler/JointPVArray.h"
+#include "dense_laser_assembler/tagged_laser_scan.h"
+
#include "calibration_msgs/DenseLaserSnapshot.h"
+
namespace dense_laser_assembler
{
-typedef LaserScanTagger<JointExtractor::JointArray> LaserJointTagger ;
-typedef LaserScanTagger<JointExtractor::JointArray>::TaggedLaserScan JointTaggedLaserScan ;
-typedef message_filters::MsgCache<JointTaggedLaserScan> JointTaggedLaserScanCache ;
+typedef LaserScanTagger<JointPVArray> LaserJointTagger ;
+typedef TaggedLaserScan<JointPVArray> JointTaggedLaserScan ;
/**
* \brief Builds a dense laser snapshot
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -0,0 +1,207 @@
+/*********************************************************************
+* 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...
+
+
+#ifndef DENSE_LASER_ASSEMBLER_DENSE_LASER_MSG_FILTER_H_
+#define DENSE_LASER_ASSEMBLER_DENSE_LASER_MSG_FILTER_H_
+
+#include "ros/ros.h"
+#include "dense_laser_assembler/joint_pv_msg_filter.h"
+#include "dense_laser_assembler/laser_scan_tagger.h"
+#include "message_filters/cache.h"
+
+
+// Messages
+#include "sensor_msgs/LaserScan.h"
+#include "mechanism_msgs/MechanismState.h"
+#include "calibration_msgs/DenseLaserSnapshot.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) ; \
+ }
+
+namespace dense_laser_assembler
+{
+
+/**
+ * \brief Listens to LaserScan and MechanismState messages, and builds DenseLaserSnapshots
+ */
+class DenseLaserMsgFilter
+{
+public:
+
+ typedef boost::shared_ptr<const TaggedLaserScan<JointPVArray> > MConstPtr ;
+ typedef boost::function<void(const MConstPtr&)> Callback;
+ typedef boost::signal<void(const MConstPtr&)> Signal;
+
+ //! \brief Not yet implemented
+ template<class A, class B>
+ DenseLaserMsgFilter(std::string name, A& a, B&b, unsigned int laser_queue_size,
+ unsigned int laser_cache_size, unsigned int mech_state_cache,
+ std::vector<std::string> joint_names) ;
+
+
+ /**
+ * \brief Construct assembler, and subscribe to the both LaserScan and MechanismState data providers
+ * \param name Namespace for the node. Filter will search for params in ~/[name]/
+ * \param laser_scan_provider MsgFilter that will provide LaserScan messages
+ * \param mech_state_provider MsgFilter that will provide MechanismState message
+ */
+ template<class A, class B>
+ DenseLaserMsgFilter(std::string name, A& laser_scan_provider, B& mech_state_provider)
+ {
+ subscribeLaserScan(laser_scan_provider) ;
+ subscribeMechState(mech_state_provider) ;
+
+ // Get all the different queue size parameters
+ CONSTRUCT_INT(laser_queue_size, 40) ;
+ CONSTRUCT_INT(laser_cache_size, 1000) ;
+ CONSTRUCT_INT(mech_state_cache_size, 100) ;
+
+ // Get the list of joints that we care about
+ joint_names_.clear() ;
+ bool found_joint = true ;
+ int joint_count = 0 ;
+
+ char param_buf[1024] ;
+ while(found_joint)
+ {
+ sprintf(param_buf, "~joint_name_%02u", joint_count) ;
+ std::string param_name = param_buf ;
+ std::string cur_joint_name ;
+ found_joint = n_.getParam(param_name, cur_joint_name) ;
+ if (found_joint)
+ {
+ ROS_INFO("[%s] -> %s", param_name.c_str(), cur_joint_name.c_str()) ;
+ joint_names_.push_back(cur_joint_name) ;
+ }
+ else
+ ROS_DEBUG("Did not find param [%s]", param_name.c_str()) ;
+ joint_count++ ;
+ }
+
+ // Configure the joint_pv_filter and associated cache
+ joint_pv_filter_.setJointNames(joint_names_) ;
+ joint_cache_.setCacheSize(mech_state_cache_size) ;
+ joint_cache_.subscribe(joint_pv_filter_) ;
+
+ // Set up the laser tagger and associated cache
+ laser_tagger_.setMaxQueueSize(laser_queue_size) ;
+ laser_tagger_.subscribeTagCache(joint_cache_) ;
+ tagged_laser_cache_.setCacheSize(laser_cache_size) ;
+ tagged_laser_cache_.subscribe(laser_tagger_) ;
+ }
+
+ /**
+ * \brief Used to connect this MessageFilter to a callback
+ * \param callback Function to be called whenever a new TaggedLaserScan message is available
+ */
+ boost::signals::connection connect(const Callback& callback) ;
+
+ //! \brief Not yet implemented
+ void processLaserScan(const sensor_msgs::LaserScanConstPtr& msg) ;
+
+ //! \brief Not yet implemented
+ void processMechState(const mechanism_msgs::MechanismState& msg) ;
+
+ //! \brief Get what the oldest time processed scan is
+ ros::Time getOldestScanTime() ;
+ //! \brief Get what the new time processed scan is
+ ros::Time getNewstScanTime() ;
+
+ /**
+ * \brief Builds a dense laser snapshot
+ * Grabs all the scans between the start and end time, and composes them into one larger snapshot. This call is non-blocking.
+ * Thus, if there are scans that haven't been cached yet, but occur before the end time, they won't be added to the snapshot.
+ * \param start The earliest scan time to be included in the snapshot
+ * \param end The latest scan time to be included in the snapshot
+ * \param output: A populated snapshot message
+ * \return True if successful. False if unsuccessful
+ */
+ bool buildSnapshotFromInterval(const ros::Time& start, const ros::Time& end, calibration_msgs::DenseLaserSnapshot& snapshot) ;
+private:
+ ros::NodeHandle n_ ;
+
+ /**
+ * \brief Subscribe to a message filter that outputs LaserScans.
+ * This is only useful if we're not subscribing to laser scans in the constructor.
+ * Thus, this will stay private until we have more flexible constructors
+ */
+ template<class A>
+ void subscribeLaserScan(A& a)
+ {
+ laser_tagger_.subscribeLaserScan(a) ;
+ }
+
+ /**
+ * \brief Subscribe to a message filter that outputs MechanismState.
+ * This is only useful if we're not subscribing to MechanismState in the constructor.
+ * Thus, this will stay private until we have more flexible constructors
+ */
+ template<class B>
+ void subscribeMechState(B& b)
+ {
+ joint_pv_filter_.subscribe(b) ;
+ }
+
+ //! Extracts positions data for a subset of joints in MechanismState
+ JointPVMsgFilter joint_pv_filter_ ;
+
+ //! Stores a time history of position data for a subset of joints in MechanismState
+ message_filters::Cache<JointPVArray> joint_cache_ ;
+
+ //! Combines a laser scan with the set of pertinent joint positions
+ LaserScanTagger< JointPVArray > laser_tagger_ ;
+
+ //! Stores a time history of laser scans that are annotated with joint positions.
+ message_filters::Cache< TaggedLaserScan<JointPVArray> > tagged_laser_cache_ ;
+
+ std::vector<std::string> joint_names_ ;
+} ;
+
+}
+
+#undef CONSTRUCT_INT
+
+#endif
Deleted: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_extractor.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -1,133 +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.
-*********************************************************************/
-
-/*! \mainpage
- * \htmlinclude manifest.html
- */
-
-#ifndef DENSE_LASER_ASSEMBLER_JOINT_EXTRACTOR
-#define DENSE_LASER_ASSEMBLER_JOINT_EXTRACTOR
-
-#include <vector>
-
-#include "mechanism_msgs/MechanismState.h"
-
-namespace dense_laser_assembler
-{
-
-/**
- * Streams an array of joint positions from MechanismState, given a list of requested joints
- */
-class JointExtractor
-{
-public:
-
- struct JointArray
- {
- roslib::Header header ;
- std::vector<double> positions ;
- } ;
-
- JointExtractor()
- {
- joint_names_.clear() ;
- }
-
- JointExtractor(std::vector<std::string> joint_names)
- {
- joint_names_ = joint_names ;
- }
-
- void setJointNames(std::vector<std::string> joint_names)
- {
- joint_names_ = joint_names ;
- }
-
- /**
- * Links Consumer's input to some provider's output.
- * \param provider The filter from which we want to receive data
- */
- template<class T>
- void subscribe(T& provider)
- {
- provider.addOutputCallback(boost::bind(&JointExtractor::processMechState, this, _1)) ;
- }
-
- void processMechState(const mechanism_msgs::MechanismStateConstPtr& msg)
- {
- //printf("Processing MechState\n") ;
- boost::shared_ptr<JointArray> joint_array_ptr(new JointArray) ;
- joint_array_ptr->positions.resize(joint_names_.size()) ;
-
- joint_array_ptr->header = msg->header ;
-
- //! \todo This can seriously be optimized using a map, or some sort of cached lookup
- for (unsigned int i=0; i<joint_names_.size(); i++)
- {
- for (unsigned int j=0; j<msg->joint_states.size(); j++)
- {
- if (joint_names_[i] == msg->joint_states[j].name )
- {
- joint_array_ptr->positions[i] = msg->joint_states[j].position ;
- break ;
- }
- }
- }
-
- // Sequentially call each registered call
- for (unsigned int i=0; i<output_callbacks_.size(); i++)
- output_callbacks_[i](joint_array_ptr) ;
- }
-
- void addOutputCallback(const boost::function<void(const boost::shared_ptr<JointArray const>&)>& callback)
- {
- output_callbacks_.push_back(callback) ;
- }
-
-private:
- std::vector<std::string> joint_names_ ;
- std::vector<boost::function<void(const boost::shared_ptr<JointArray const>&)> > output_callbacks_ ;
-
-} ;
-
-}
-
-
-
-
-
-
-
-
-#endif // DENSE_LASER_ASSEMBLER_JOINT_EXTRACTOR
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -0,0 +1,168 @@
+/*********************************************************************
+* 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 DENSE_LASER_ASSEMBLER_JOINT_PV_MSG_FILTER_H_
+#define DENSE_LASER_ASSEMBLER_JOINT_PV_MSG_FILTER_H_
+
+#include <vector>
+
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/signals.hpp>
+
+// Messages
+#include "dense_laser_assembler/JointPVArray.h"
+#include "mechanism_msgs/MechanismState.h"
+
+namespace dense_laser_assembler
+{
+
+/**
+ * Streams an array of joint positions and velocities from MechanismState,
+ * given a mapping from joint_names to array indices
+ */
+class JointPVMsgFilter
+{
+public:
+ typedef boost::function<void(const JointPVArrayConstPtr&)> Callback;
+ typedef boost::signal<void(const JointPVArrayConstPtr&)> Signal;
+
+ /**
+ * \brief Subscribe to another MessageFilter at construction time
+ * \param a The parent message filter
+ * \param joint_names Vector of joint names that we want to output. Must match
+ * joint names in mechanism state
+ */
+ template<class A>
+ JointPVMsgFilter(A& a, std::vector<std::string> joint_names = std::vector<std::string>())
+ {
+ setJointNames(joint_names);
+ subscribe(a);
+ }
+
+ /**
+ * \brief Construct without subcribing to another MsgFilter at construction time
+ * \param joint_names Vector of joint names that we want to output. Must match
+ * joint names in mechanism state
+ */
+ JointPVMsgFilter(std::vector<std::string> joint_names = std::vector<std::string>())
+ {
+ setJointNames(joint_names);
+ }
+
+ /**
+ * \brief Subcribes this MsgFilter to another MsgFilter
+ * \param a The MsgFilter that this MsgFilter should get data from
+ */
+ template<class A>
+ void subscribe(A& a)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_connection_ = a.connect(boost::bind(&JointPVMsgFilter::processMechState, this, _1));
+ }
+
+ /**
+ * \brief Called by user wants they want to connect this MessageFilter to their own code
+ * \param callback Function to be called whenever a JointPVArray message is available
+ */
+ boost::signals::connection connect(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return signal_.connect(callback);
+ }
+
+ /**
+ * \brief Define the mapping from MechanismState to JointPVArray
+ */
+ void setJointNames(std::vector<std::string> joint_names)
+ {
+ boost::mutex::scoped_lock lock(joint_mapping_mutex_);
+ joint_names_ = joint_names;
+ }
+
+ /**
+ * \brief Extracts joint positions from MechState, using the joint_names mapping.
+ * Also calls all the callback functions as set by connect()
+ * \param msg The MechanismState message from which we want to extract positions
+ */
+ void processMechState(const mechanism_msgs::MechanismStateConstPtr& msg)
+ {
+ // Allocate mem to store out output data
+ boost::shared_ptr<JointPVArray> joint_array_ptr(new JointPVArray) ;
+
+ // Copy MechState data into a JointPVArray
+ {
+ boost::mutex::scoped_lock lock(joint_mapping_mutex_);
+
+ joint_array_ptr->pos.resize(joint_names_.size()) ;
+ joint_array_ptr->vel.resize(joint_names_.size()) ;
+
+ joint_array_ptr->header = msg->header ;
+
+ //! \todo This can seriously be optimized using a map, or some sort of cached lookup
+ for (unsigned int i=0; i<joint_names_.size(); i++)
+ {
+ for (unsigned int j=0; j<msg->joint_states.size(); j++)
+ {
+ if (joint_names_[i] == msg->joint_states[j].name )
+ {
+ joint_array_ptr->pos[i] = msg->joint_states[j].position ;
+ joint_array_ptr->vel[i] = msg->joint_states[j].velocity ;
+ break ;
+ }
+ }
+ }
+ }
+
+ // Call all connected callbacks
+ signal_(joint_array_ptr) ;
+ }
+
+protected:
+ boost::mutex joint_mapping_mutex_ ;
+ std::vector<std::string> joint_names_ ;
+
+ // Filter Connection Stuff
+ boost::signals::connection incoming_connection_;
+ Signal signal_;
+ boost::mutex signal_mutex_;
+} ;
+
+}
+
+#endif // DENSE_LASER_ASSEMBLER_JOINT_PV_MSG_FILTER_H_
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -41,87 +41,138 @@
#include <deque>
#include "sensor_msgs/LaserScan.h"
-#include "message_filters/msg_cache.h"
-#include "dense_laser_assembler/joint_extractor.h"
+#include "message_filters/cache.h"
#include "boost/shared_ptr.hpp"
+#include "dense_laser_assembler/tagged_laser_scan.h"
namespace dense_laser_assembler
{
+/**
+ * Listens to LaserScans and a cache of 'tag' messages. Outputs a laserScan
+ * whenever there is a tag msg that occurs before and after the scan.
+ */
template <class T>
class LaserScanTagger
{
public:
- struct TaggedLaserScan
- {
- roslib::Header header ;
- sensor_msgs::LaserScanConstPtr scan ;
- boost::shared_ptr<const T> before ;
- boost::shared_ptr<const T> after ;
- } ;
+ typedef boost::shared_ptr<const T> TConstPtr ;
+ typedef boost::shared_ptr<const TaggedLaserScan<T> > MConstPtr ;
+ typedef boost::function<void(const MConstPtr&)> Callback;
+ typedef boost::signal<void(const MConstPtr&)> Signal;
- typedef boost::shared_ptr<const TaggedLaserScan> TaggedLaserScanConstPtr ;
- LaserScanTagger(message_filters::MsgCache<T>& tag_cache, unsigned int max_queue_size)
+ template<class A>
+ /**
+ * \brief Construct object, and also subscribe to relevant data sources
+ */
+ LaserScanTagger(A& a, message_filters::Cache<T>& tag_cache, unsigned int max_queue_size)
{
+ subscribeLaserScan(a);
+ subscribeTagCache(tag_cache);
+ setMaxQueueSize(max_queue_size);
tag_cache_ = &tag_cache ;
max_queue_size_ = max_queue_size ;
}
LaserScanTagger()
{
- tag_cache_ = NULL ;
- max_queue_size_ = 1 ;
+ tag_cache_ = NULL;
+ setMaxQueueSize(1);
}
- void setQueueSize(unsigned int max_queue_size)
+ ~LaserScanTagger()
{
- max_queue_size_ = max_queue_size ;
+ incoming_laser_scan_connection_.disconnect() ;
+ incoming_tag_connection_.disconnect() ;
}
- void setTagCache(message_filters::MsgCache<T>& tag_cache)
+ template<class A>
+ void subscribeLaserScan(A& a)
{
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ incoming_laser_scan_connection_ = a.connect(boost::bind(&LaserScanTagger<T>::processLaserScan, this, _1));
+ }
+
+ void subscribeTagCache(message_filters::Cache<T>& tag_cache)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
tag_cache_ = &tag_cache ;
+ incoming_tag_connection_ = tag_cache.connect(boost::bind(&LaserScanTagger<T>::processTag, this, _1));
}
+ void setMaxQueueSize(unsigned int max_queue_size)
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+ max_queue_size_ = max_queue_size;
+ }
+
/**
- * Adds the laser scan onto the queue of scans that need to be matched with the stamped data
+ * Adds the laser scan onto the queue of scans that need to be matched with the tags
*/
void processLaserScan(const sensor_msgs::LaserScanConstPtr& msg)
{
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+ //! \todo need to more carefully decide on overflow logic
+ if (queue_.size() < max_queue_size_)
+ queue_.push_back(msg) ;
+ else
+ ROS_WARN("Queue full, not pushing new data onto queue until queue is serviced") ;
+ }
- queue_lock_.lock() ;
+ update() ;
+ }
- //! \todo need to decide on overflow logic
- if (queue_.size() < max_queue_size_)
- queue_.push_back(msg) ;
- else
- ROS_WARN("Queue full, not pushing new data onto queue until queue is serviced") ;
- queue_lock_.unlock() ;
-
+ /**
+ * Since we just received a new tag message, we might have laser scans that we need to process
+ */
+ void processTag(const TConstPtr& msg)
+ {
update() ;
}
/**
+ * \brief Connect this message filter's output to some callback
+ * \param callback Function to call after we've tagged a LaserScan
+ */
+ boost::signals::connection connect(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return signal_.connect(callback);
+ }
+
+ /**
* Triggers the module to try to service the queue
*/
void update()
{
+ //! \todo This is not a good enough check. We need to somehow make sure the cache hasn't been destructed
if (!tag_cache_)
{
- ROS_WARN("Have a NULL pointer to TagCache. Skipping update") ;
- return ;
+ ROS_WARN("Have a NULL pointer to TagCache. Skipping update");
+ return;
}
- queue_lock_.lock() ;
-
bool did_something = true ;
- while (did_something && queue_.size() > 0)
+ // It's possible we need to process multiple laser scans. Therefore, keep
+ // looping until we reach a scan that's a no-op
+ while (true)
{
+ //! \todo Come up with better locking/unlocking in this loop
+ queue_mutex_.lock();
+
+ // Exit condition
+ if (!did_something || queue_.size() == 0)
+ {
+ queue_mutex_.unlock();
+ break;
+ }
+
did_something = false ; // Haven't done anything yet
const sensor_msgs::LaserScanConstPtr& elem = queue_.front() ;
@@ -132,51 +183,45 @@
boost::shared_ptr<const T> tag_before = tag_cache_->getElemBeforeTime(scan_start) ;
boost::shared_ptr<const T> tag_after = tag_cache_->getElemAfterTime(scan_end) ;
+
if (!tag_before) // Can't get old enough tag. Give up
{
queue_.pop_front() ;
did_something = true ;
- ROS_WARN("Popped elem off back of queue") ;
+ queue_mutex_.unlock() ;
}
else if (!tag_after) // Don't have new enough tag. Keep scan on queue until we get it
{
did_something = false ;
+ queue_mutex_.unlock() ;
}
else // Both tags are fine. Use the data, and push out
{
- did_something = true ;
- boost::shared_ptr<TaggedLaserScan> tagged_scan(new TaggedLaserScan) ;
+ did_something = true;
+ boost::shared_ptr<TaggedLaserScan<T> > tagged_scan(new TaggedLaserScan<T> ) ;
tagged_scan->header = elem->header ;
tagged_scan->scan = elem ;
tagged_scan->before = tag_before ;
tagged_scan->after = tag_after ;
+ queue_.pop_front() ;
+ queue_mutex_.unlock() ;
- for (unsigned int i=0; i<output_callbacks_.size(); i++)
- output_callbacks_[i](tagged_scan) ;
-
- queue_.pop_front() ;
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_(tagged_scan) ;
}
-
}
-
- queue_lock_.unlock() ;
}
- void addOutputCallback(const boost::function<void(const TaggedLaserScanConstPtr&)>& callback)
- {
- output_callbacks_.push_back(callback) ;
- }
-
-
private:
std::deque<sensor_msgs::LaserScanConstPtr> queue_ ; //!< Incoming queue of laser scans
- boost::mutex queue_lock_ ; //!< Mutex for queue
- unsigned int max_queue_size_ ; //!< Max # of laser scans to queue up for processing
+ boost::mutex queue_mutex_ ; //!< Mutex for laser scan queue
+ unsigned int max_queue_size_ ; //!< Max # of laser scans to queue up for processing
+ message_filters::Cache<T>* tag_cache_ ; //!< Cache of the tags that we need to merge with laser data
- message_filters::MsgCache<T>* tag_cache_ ; //!< Cache of the tags that we need to merge with laser data
-
- std::vector<boost::function<void(const TaggedLaserScanConstPtr&)> > output_callbacks_ ;
-
+ Signal signal_;
+ boost::signals::connection incoming_laser_scan_connection_;
+ boost::signals::connection incoming_tag_connection_;
+ boost::mutex signal_mutex_;
} ;
}
Copied: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h (from rev 18311, pkg/trunk/sandbox/message_filters/src/filter_example.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -0,0 +1,54 @@
+/*********************************************************************
+* 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 DENSE_LASER_ASSEMBLER_TAGGED_LASER_SCAN_H_
+#define DENSE_LASER_ASSEMBLER_TAGGED_LASER_SCAN_H_
+
+
+namespace dense_laser_assembler
+{
+
+template <class T>
+struct TaggedLaserScan
+{
+ roslib::Header header ;
+ sensor_msgs::LaserScanConstPtr scan ;
+ boost::shared_ptr<const T> before ;
+ boost::shared_ptr<const T> after ;
+} ;
+
+}
+
+
+#endif // DENSE_LASER_ASSEMBLER_TAGGED_LASER_SCAN_H_
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-07-06 17:22:54 UTC (rev 18313)
@@ -20,6 +20,9 @@
<depend package="mechanism_msgs" />
<depend package="message_filters" />
<depend package="calibration_msgs" />
+
+ <depend package="diagnostic_updater" />
+ <depend package="diagnostic_msgs" />
<export>
<cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp -I{prefix}/include" />
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/msg/JointPVArray.msg
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/msg/JointPVArray.msg (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/msg/JointPVArray.msg 2009-07-06 17:22:54 UTC (rev 18313)
@@ -0,0 +1,4 @@
+# Stores a vector of positions
+Header header
+float64[] pos
+float64[] vel
\ No newline at end of file
Modified: 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 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/build_dense_laser_snapshot.cpp 2009-07-06 17:22:54 UTC (rev 18313)
@@ -62,7 +62,8 @@
snapshot.joint_encoding = 0 ;
snapshot.joint_names.clear() ;
snapshot.joint_positions.clear() ;
- //snapshot.scan_start.clear() ;
+ snapshot.joint_velocities.clear() ;
+ snapshot.scan_start.clear() ;
return true ;
}
@@ -98,9 +99,10 @@
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)
+ snapshot.joint_velocities.resize(num_joints*h*2) ;
// Set up other data vectors
- //snapshot.scan_start.resize(h) ;
+ snapshot.scan_start.resize(h) ;
snapshot.ranges.resize(w*h) ;
snapshot.intensities.resize(w*h) ;
@@ -112,18 +114,20 @@
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
+ // Copy joint positions & velocities
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] ;
+ snapshot.joint_positions[i*j*2 + 0] = scans[i]->before->pos[j] ;
+ snapshot.joint_positions[i*j*2 + 1] = scans[i]->after->pos[j] ;
+ snapshot.joint_velocities[i*j*2 + 0] = scans[i]->before->vel[j] ;
+ snapshot.joint_velocities[i*j*2 + 1] = scans[i]->after->vel[j] ;
}
// Copy time stamp
- //snapshot.scan_start[i] = scans[i]->header.stamp ;
+ snapshot.scan_start[i] = scans[i]->header.stamp ;
}
- ROS_INFO("Done building snapshot that is [%u rows] x [%u cols]\n", h, w) ;
+ ROS_DEBUG("Done building snapshot that is [%u rows] x [%u cols]", h, w) ;
return true ;
}
Modified: 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 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_snapshotter.cpp 2009-07-06 17:22:54 UTC (rev 18313)
@@ -34,9 +34,10 @@
#include "ros/ros.h"
-// Services
-#include "dense_laser_assembler/BuildLaserSnapshot.h"
+#include "message_filters/subscriber.h"
+#include "dense_laser_assembler/dense_laser_msg_filter.h"
+
// Messages
#include "calibration_msgs/DenseLaserSnapshot.h"
#include "pr2_msgs/LaserScannerSignal.h"
@@ -44,18 +45,22 @@
using namespace dense_laser_assembler ;
+/**
+ * \brief Builds a DenseLaserSnapshot message from laser scans collected in a specified time interval
+ */
class DenseLaserSnapshotter
{
public:
- DenseLaserSnapshotter()
+ DenseLaserSnapshotter() :
+ scan_sub_(n_, "tilt_scan", 20),
+ mech_state_sub_(n_, "mechanism_state", 20),
+ dense_laser_filter_("dense_laser_filter", scan_sub_, mech_state_sub_)
{
prev_signal_.header.stamp.fromNSec(0) ;
-
+ signal_sub_ = n_.subscribe("laser_tilt_controller/laser_scanner_signal", 2, &DenseLaserSnapshotter::scannerSignalCallback, this) ;
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 ;
}
@@ -66,6 +71,7 @@
void scannerSignalCallback(const pr2_msgs::LaserScannerSignalConstPtr& cur_signal)
{
+ ROS_DEBUG("Got Scanner 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 ;
@@ -77,46 +83,40 @@
}
else
{
- printf("About to make request\n") ;
+ if (cur_signal->signal == 1)
+ {
+ ROS_DEBUG("About to make request") ;
- BuildLaserSnapshot::Request req ;
- BuildLaserSnapshot::Response resp ;
+ ros::Time start = prev_signal_.header.stamp ;
+ ros::Time end = cur_signal->header.stamp ;
- req.start = prev_signal_.header.stamp ;
- req.end = cur_signal->header.stamp ;
+ calibration_msgs::DenseLaserSnapshot snapshot ;
+ dense_laser_filter_.buildSnapshotFromInterval(start, end, snapshot) ;
+ ROS_DEBUG("header.stamp: %f", snapshot.header.stamp.toSec()) ;
+ ROS_DEBUG("header.frame_id: %s", snapshot.header.frame_id.c_str()) ;
+ ROS_DEBUG("ranges.size()=%u", snapshot.ranges.size()) ;
+ ROS_DEBUG("intensities.size()=%u", snapshot.intensities.size()) ;
+ ROS_DEBUG("joint_positions.size()=%u", snapshot.joint_positions.size()) ;
+ ROS_DEBUG("joint_velocities.size()=%u", snapshot.joint_velocities.size()) ;
+ ROS_DEBUG("scan_start.size()=%u", snapshot.scan_start.size()) ;
+ snapshot_pub_.publish(snapshot) ;
- 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") ;
-
+ }
+ else
+ ROS_DEBUG("Not making request") ;
prev_signal_ = *cur_signal ;
}
}
private:
ros::NodeHandle n_ ;
+ ros::Publisher snapshot_pub_ ;
ros::Subscriber signal_sub_ ;
- ros::Publisher snapshot_pub_ ;
-
+ message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_ ;
+ message_filters::Subscriber<mechanism_msgs::MechanismState> mech_state_sub_ ;
+ DenseLaserMsgFilter dense_laser_filter_ ;
pr2_msgs::LaserScannerSignal prev_signal_;
bool first_time_ ;
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp 2009-07-06 17:22:54 UTC (rev 18313)
@@ -36,25 +36,40 @@
* \htmlinclude manifest.html
*/
+#include <boost/signals.hpp>
+
#include "ros/ros.h"
-#include "dense_laser_assembler/joint_extractor.h"
+#include "dense_laser_assembler/joint_pv_msg_filter.h"
+//#include "dense_laser_assembler/joint_pv_diag_msg_filter.h"
+#include "dense_laser_assembler/JointPVArray.h"
+#include "diagnostic_updater/diagnostic_updater.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)
+void display_joints(vector<string> joint_names, const JointPVArrayConstPtr& 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]) ;
+ printf(" %s: %f, %f\n", joint_names[i].c_str(), joint_array_ptr->pos[i], joint_array_ptr->vel[i]) ;
}
}
+void diagnosticsLoop(diagnostic_updater::Updater* diagnostic)
+{
+ ros::NodeHandle nh ;
+ while(nh.ok())
+ {
+ diagnostic->update() ;
+ sleep(1) ;
+ }
+}
+
+
int main(int argc, char** argv)
{
ros::init(argc, argv, "joint_extractor_display") ;
@@ -65,14 +80,21 @@
joint_names.push_back("laser_tilt_mount_joint") ;
joint_names.push_back("torso_lift_link") ;
- JointExtractor joint_extractor(joint_names) ;
+ diagnostic_updater::Updater diagnostic(nh) ;
- joint_extractor.addOutputCallback(boost::bind(&display_joints, joint_names, _1) ) ;
+ JointPVMsgFilter joint_extractor(joint_names) ;
+ //JointPVDiagMsgFilter joint_extractor(diagnostic, joint_names) ;
- ros::Subscriber sub = nh.subscribe("mechanism_state", 1, &JointExtractor::processMechState, &joint_extractor) ;
+ boost::signals::connection con = joint_extractor.connect( boost::bind(&display_joints, joint_names, _1) ) ;
+ ros::Subscriber sub = nh.subscribe("mechanism_state", 1, &JointPVMsgFilter::processMechState, &joint_extractor) ;
+
+ //boost::thread* diagnostic_thread ;
+ //diagnostic_thread = new boost::thread( boost::bind(&diagnosticsLoop, &diagnostic) );
+
ros::spin() ;
+ con.disconnect() ;
return 0 ;
}
Copied: pkg/trunk/sandbox/message_filters/include/message_filters/cache.h (from rev 18311, pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h)
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/cache.h (rev 0)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/cache.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -0,0 +1,255 @@
+/*********************************************************************
+* 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_CACHE_H_
+#define MESSAGE_FILTERS_CACHE_H_
+
+#include <deque>
+#include "boost/thread.hpp"
+#include "boost/shared_ptr.hpp"
+#include <boost/signals.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 Cache
+{
+public:
+ typedef boost::shared_ptr<M const> MConstPtr ;
+ typedef boost::function<void(const MConstPtr&)> Callback;
+ typedef boost::signal<void(const MConstPtr&)> Signal;
+
+ template<class A>
+ Cache(A& a, unsigned int cache_size = 1)
+ {
+ setCacheSize(cache_size) ;
+ subscribe(a) ;
+ }
+
+ /**
+ * Initializes a Messsage Cache without specifying a parent filter. This implies that in
+ * order to populate the cache, the user then has to call addToCache themselves
+ */
+ Cache(unsigned int cache_size = 1)
+ {
+ setCacheSize(cache_size);
+ }
+
+ template<class A>
+ void subscribe(A& a)
+ {
+ incoming_connection_ = a.connect(boost::bind(&Cache::addToCache, this, _1));
+ }
+
+ ~Cache()
+ {
+ incoming_connection_.disconnect();
+ }
+
+ /**
+ * 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 ;
+ }
+
+ boost::signals::connection connect(const Callback& callback)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ return signal_.connect(callback);
+ }
+
+ /**
+ * 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 MConstPtr& msg)
+ {
+ //printf(" Cache Size: %u\n", cache_.size()) ;
+ {
+ boost::mutex::scoped_lock lock(cache_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
+ }
+
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ // Sequentially call each registered call
+ signal_(msg);
+ }
+ }
+
+ /**
+ * 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<MConstPtr> getInterval(const ros::Time& start, const ros::Time& end)
+ {
+ boost::mutex::scoped_lock lock(cache_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<MConstPtr> 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]) ;
+ }
+
+ 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
+ */
+ MConstPtr getElemBeforeTime(const ros::Time& time)
+ {
+ boost::mutex::scoped_lock lock(cache_lock_);
+
+ MConstPtr out ;
+
+ 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] ;
+
+ 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
+ */
+ MConstPtr getElemAfterTime(const ros::Time& time)
+ {
+ boost::mutex::scoped_lock lock(cache_lock_);
+
+ MConstPtr out ;
+
+ 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() ;
+
+ return out ;
+ }
+
+ /**
+ * Returns the timestamp associated with the newest packet cache
+ */
+ ros::Time getLatestTime()
+ {
+ boost::mutex::scoped_lock lock(cache_lock_);
+
+ ros::Time latest_time(0, 0) ;
+
+ if (cache_.size() > 0)
+ latest_time = cache_.back()->header.stamp ;
+
+ return latest_time ;
+ }
+
+private:
+ boost::mutex cache_lock_ ; //!< Lock for cache_
+ std::deque<MConstPtr > cache_ ; //!< Cache for the messages
+ unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache.
+
+ boost::signals::connection incoming_connection_;
+ Signal signal_;
+ boost::mutex signal_mutex_;
+};
+
+}
+
+
+#endif /* MESSAGE_FILTERS_CACHE_H_ */
Deleted: pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h 2009-07-06 17:15:23 UTC (rev 18312)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/msg_cache.h 2009-07-06 17:22:54 UTC (rev 18313)
@@ -1,249 +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.
-*********************************************************************/
-
-/*! \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 <boost/signals.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> MConstPtr ;
- typedef boost::function<void(const MConstPtr&)> Callback;
- typedef boost::signal<void(const MConstPtr&)> Signal;
-
- template<class A>
- MsgCache(A& a, unsigned int cache_size = 1)
- {
- setCacheSize(cache_size) ;
- incoming_connection_ = a.connect(boost::bind(&MsgCache::addToCache, this, _1));
- }
-
- /**
- * Initializes a MsgCache without specifying a parent filter. This implies that in
- * order to populate the cache, the user then has to call addToCache themselves
- */
- MsgCache(unsigned int cache_size = 1)
- {
- setCacheSize(cache_size);
- }
-
- ~MsgCache()
- {
- incoming_connection_.disconnect();
- }
-
- /**
- * 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 ;
- }
-
- boost::signals::connection connect(const Callback& callback)
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(callback);
- }
-
- /**
- * 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 MConstPtr& msg)
- {
- //printf(" Cache Size: %u\n", cache_.size()) ;
- {
- boost::mutex::scoped_lock lock(cache_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
- }
-
- {
- boost::mutex::scoped_lock lock(signal_mutex_);
- // Sequentially call each registered call
- signal_(msg);
- }
- }
-
- /**
- * 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<MConstPtr> getInterval(const ros::Time& start, const ros::Time& end)
- {
- boost::mutex::scoped_lock lock(cache_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_...
[truncated message content] |
