Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23514] pkg/trunk
|
From: <jl...@us...> - 2009-09-01 08:29:32
|
Revision: 23514
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23514&view=rev
Author: jleibs
Date: 2009-09-01 08:29:22 +0000 (Tue, 01 Sep 2009)
Log Message:
-----------
Also checking in contents for pr2_laser_snapshotter and dense_laser_assembler
Added Paths:
-----------
pkg/trunk/calibration_experimental/dense_laser_assembler/CMakeLists.txt
pkg/trunk/calibration_experimental/dense_laser_assembler/Makefile
pkg/trunk/calibration_experimental/dense_laser_assembler/ROS_BUILD_BLACKLIST
pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h
pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h
pkg/trunk/calibration_experimental/dense_laser_assembler/mainpage.dox
pkg/trunk/calibration_experimental/dense_laser_assembler/manifest.xml
pkg/trunk/calibration_experimental/dense_laser_assembler/msg/Float32MultiArrayStamped.msg
pkg/trunk/calibration_experimental/dense_laser_assembler/msg/JointPVArray.msg
pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/assembler_node.py
pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/laser_image_node.py
pkg/trunk/calibration_experimental/dense_laser_assembler/src/build_dense_laser_snapshot.cpp
pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/__init__.py
pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler_srv.cpp
pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_msg_filter.cpp
pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_snapshotter.cpp
pkg/trunk/calibration_experimental/dense_laser_assembler/src/joint_extractor_display.cpp
pkg/trunk/calibration_experimental/dense_laser_assembler/src/laser_imager.cpp
pkg/trunk/calibration_experimental/dense_laser_assembler/src/tagged_laser_cache_display.cpp
pkg/trunk/calibration_experimental/dense_laser_assembler/srv/BuildLaserSnapshot.srv
pkg/trunk/pr2/pr2_laser_snapshotter/CMakeLists.txt
pkg/trunk/pr2/pr2_laser_snapshotter/Makefile
pkg/trunk/pr2/pr2_laser_snapshotter/mainpage.dox
pkg/trunk/pr2/pr2_laser_snapshotter/manifest.xml
pkg/trunk/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp
pkg/trunk/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp
pkg/trunk/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv
pkg/trunk/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp
pkg/trunk/pr2/pr2_laser_snapshotter/test/test_assembler.cpp
pkg/trunk/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/CMakeLists.txt 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE Debug)
+
+rospack(dense_laser_assembler)
+
+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(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)
+
+#rospack_add_executable(talker src/talker.cpp)
+#rospack_add_executable(listener src/listener.cpp)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+genmsg()
+gensrv()
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/Makefile
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/Makefile (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/Makefile 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/ROS_BUILD_BLACKLIST 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1 @@
+
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,76 @@
+/*********************************************************************
+* 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_BUILD_DENSE_LASER_SNAPSHOT_H_
+#define DENSE_LASER_ASSEMBLER_BUILD_DENSE_LASER_SNAPSHOT_H_
+
+
+#include <vector>
+#include "dense_laser_assembler/laser_scan_tagger.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<JointPVArray> LaserJointTagger ;
+typedef TaggedLaserScan<JointPVArray> JointTaggedLaserScan ;
+
+/**
+ * \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
+ */
+
+bool buildDenseLaserSnapshot(const std::vector<boost::shared_ptr<const JointTaggedLaserScan> >& scans,
+ const std::vector<std::string>& joint_names,
+ calibration_msgs::DenseLaserSnapshot& snapshot) ;
+
+
+}
+
+
+#endif // DENSE_LASER_ASSEMBLER_BUILD_DENSE_LASER_SNAPSHOT
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,201 @@
+/*********************************************************************
+* 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"
+#include "message_filters/connection.h"
+#include "message_filters/simple_filter.h"
+
+
+// Messages
+#include "sensor_msgs/LaserScan.h"
+#include "pr2_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 message_filters::SimpleFilter<TaggedLaserScan<JointPVArray> >
+{
+public:
+
+ typedef boost::shared_ptr<const TaggedLaserScan<JointPVArray> > MConstPtr ;
+
+ //! \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_.connectInput(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_.connectInput(laser_tagger_) ;
+ }
+
+ //! \brief Not yet implemented
+ void processLaserScan(const sensor_msgs::LaserScanConstPtr& msg) ;
+
+ //! \brief Not yet implemented
+ void processMechState(const pr2_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_.connectInput(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
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,155 @@
+/*********************************************************************
+* 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>
+#include <boost/bind.hpp>
+#include <message_filters/connection.h>
+#include <message_filters/simple_filter.h>
+
+// Messages
+#include "dense_laser_assembler/JointPVArray.h"
+#include "pr2_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 message_filters::SimpleFilter<JointPVArray>
+{
+public:
+ /**
+ * \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 connectInput(A& a)
+ {
+ incoming_connection_ = a.registerCallback(boost::bind(&JointPVMsgFilter::processMechState, this, _1));
+ }
+
+ /**
+ * \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 pr2_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
+ signalMessage(joint_array_ptr) ;
+ }
+
+protected:
+ boost::mutex joint_mapping_mutex_ ;
+ std::vector<std::string> joint_names_ ;
+
+ // Filter Connection Stuff
+ message_filters::Connection incoming_connection_;
+} ;
+
+}
+
+#endif // DENSE_LASER_ASSEMBLER_JOINT_PV_MSG_FILTER_H_
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,217 @@
+/*********************************************************************
+* 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_LASER_SCAN_TAGGER_H_
+#define DENSE_LASER_ASSEMBLER_LASER_SCAN_TAGGER_H_
+
+#include <deque>
+#include "sensor_msgs/LaserScan.h"
+#include "message_filters/cache.h"
+#include "boost/shared_ptr.hpp"
+#include "dense_laser_assembler/tagged_laser_scan.h"
+
+#include <message_filters/simple_filter.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 message_filters::SimpleFilter<TaggedLaserScan<T> >
+{
+public:
+
+ typedef boost::shared_ptr<const T> TConstPtr ;
+ typedef boost::shared_ptr<const TaggedLaserScan<T> > MConstPtr ;
+
+
+
+ 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;
+ setMaxQueueSize(1);
+ }
+
+ ~LaserScanTagger()
+ {
+ incoming_laser_scan_connection_.disconnect() ;
+ incoming_tag_connection_.disconnect() ;
+ }
+
+ template<class A>
+ void subscribeLaserScan(A& a)
+ {
+ incoming_laser_scan_connection_ = a.registerCallback(boost::bind(&LaserScanTagger<T>::processLaserScan, this, _1));
+ }
+
+ void subscribeTagCache(message_filters::Cache<T>& tag_cache)
+ {
+ tag_cache_ = &tag_cache ;
+ incoming_tag_connection_ = tag_cache.registerCallback(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 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") ;
+ }
+
+ update() ;
+ }
+
+
+ /**
+ * Since we just received a new tag message, we might have laser scans that we need to process
+ */
+ void processTag(const TConstPtr& msg)
+ {
+ update() ;
+ }
+
+ /**
+ * 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;
+ }
+
+ bool did_something = true ;
+
+ // 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() ;
+
+ ros::Time scan_start = elem->header.stamp ;
+ ros::Time scan_end = elem->header.stamp + ros::Duration().fromSec(elem->time_increment*elem->ranges.size()) ;
+
+ 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 ;
+ 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<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() ;
+
+ signalMessage(tagged_scan) ;
+ }
+ }
+ }
+
+private:
+
+ std::deque<sensor_msgs::LaserScanConstPtr> queue_ ; //!< Incoming queue of laser scans
+ 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::Connection incoming_laser_scan_connection_;
+ message_filters::Connection incoming_tag_connection_;
+} ;
+
+}
+
+
+
+#endif /* DENSE_LASER_ASSEMBLER_LASER_SCAN_TAGGER_H_ */
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h 2009-09-01 08:29:22 UTC (rev 23514)
@@ -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_
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/mainpage.dox
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/mainpage.dox (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/mainpage.dox 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,28 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\section dense_laser_assembler
+\b dense_laser_assembler Aggregates laser data very similarly to the
+point cloud assembler. However, the output data structures are very
+different. Instead of defining each laser ray as an XYZ point with an
+intensity, the dense_laser_assembler instead uses multiarrays to store
+the scans in a dense representation.
+
+Intensity and Range data are each stored in their own Float32
+multiarray. Each row of the multiarray is a scan, and these rows
+are stacked (sorted by the tilting angle) to form the 'rasterized'
+dense representation.
+
+Meta information is also stored along with the range and intensity data.
+The tilting angle of the hokuyo is published as another multiarray. And
+the laser's configuration params are also published. This then provides
+us enough information to fully determine the location and time of any
+laser ray in the dense representation.
+
+\section laser_image_node
+laser_image_node converts the intensity and range multiarrays into
+uint8 ROS images that can be viewed with image_view. This can often
+be a very useful debugging tool to make sure that the dense scans
+look reasonable.
+*/
\ No newline at end of file
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/manifest.xml
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/manifest.xml (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/manifest.xml 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,33 @@
+<package>
+ <description brief="Build dense laser range and intensity clouds">
+ Stores data from the PR2's tilting Hokuyo laser rangefinder, in a
+ dense representation. This allows the user to do 'image-like'
+ processing on the data, and can also be used for very fast approx
+ neighborhood searches.
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="experimental" notes=""/>
+ <!--url>http://pr.willowgarage.com/wiki/dense_laser_assembler</url-->
+
+ <depend package="roscpp"/>
+ <depend package="std_msgs"/>
+ <depend package="pr2_mechanism_msgs" />
+ <depend package="sensor_msgs"/>
+ <depend package="pr2_msgs"/>
+ <depend package="sensor_msgs" />
+ <depend package="pr2_mechanism_msgs" />
+ <depend package="message_filters" />
+ <depend package="calibration_msgs" />
+ <depend package="rospy" />
+
+ <depend package="diagnostic_updater" />
+ <depend package="diagnostic_msgs" />
+
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp -I{prefix}/include" />
+ </export>
+
+</package>
+
+
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/msg/Float32MultiArrayStamped.msg
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/msg/Float32MultiArrayStamped.msg (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/msg/Float32MultiArrayStamped.msg 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,2 @@
+Header header
+std_msgs/Float32MultiArray data
\ No newline at end of file
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/msg/JointPVArray.msg
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/msg/JointPVArray.msg (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/msg/JointPVArray.msg 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,4 @@
+# Stores a vector of positions
+Header header
+float64[] pos
+float64[] vel
\ No newline at end of file
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/assembler_node.py
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/assembler_node.py (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/assembler_node.py 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,188 @@
+#!/usr/bin/env python
+
+# 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 Willow Garage, Inc. 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.
+#
+
+
+import roslib; roslib.load_manifest('dense_laser_assembler')
+import sys
+import rospy
+import threading
+from time import sleep
+from sensor_msgs.msg import LaserScan
+from pr2_msgs import LaserScannerSignal
+from mechanism_msgs.msg import MechanismState
+from std_msgs.msg import *
+from dense_laser_assembler.msg import *
+from pr2_mechanism_controllers.msg import *
+from dense_laser_assembler.dense_laser_cache import DenseLaserCache
+from sensor_msgs.msg import *
+
+# Globals
+laser_cache = [ ]
+first_signal = True
+prev_signal_time = [ ]
+image_pub = [ ]
+test_pub = [ ]
+
+
+def scan_callback(msg) :
+ req_lock.acquire()
+ laser_cache.add_scan(msg)
+ laser_cache.process_scans()
+ laser_cache.process_interval_reqs()
+ req_lock.release()
+
+def mech_callback(msg) :
+ req_lock.acquire()
+ laser_cache.add_mech_state(msg)
+ laser_cache.process_scans()
+ laser_cache.process_interval_reqs()
+ req_lock.release()
+
+def signal_callback(msg) :
+ req_lock.acquire()
+ global first_signal
+ global prev_signal
+ laser_cache.process_scans()
+ if first_signal :
+ first_signal = False
+ else :
+ laser_cache.add_interval_req(prev_signal.header.stamp, msg.header.stamp)
+ laser_cache.process_interval_reqs()
+ prev_signal = msg
+ print 'Got signal message!'
+ print ' Signal=%i' % msg.signal
+ req_lock.release()
+
+def interval_req_callback(scans) :
+
+ # Check data consistency
+ if (len(scans) == 0) :
+ print 'Processed interval with no scans. Not publishing'
+ return
+
+ rows = len(scans)
+ cols = len(scans[0].scan.ranges)
+ if any( [len(x.scan.ranges) != cols for x in scans] ) or any( [len(x.scan.intensities) != cols for x in scans]) :
+ print "# Readings aren't consistent across interval"
+ return
+
+
+ print 'About to process cloud with %u scans' % len(scans)
+
+ # Sort by tilting joint angle
+ sorted_scans = sorted(scans, lambda x,y:cmp(x.pos[0],y.pos[0]))
+
+
+ msg_header = roslib.msg.Header()
+ msg_header.stamp = scans[-1].scan.header.stamp
+ msg_header.frame_id = scans[-1].scan.header.frame_id
+
+ layout = MultiArrayLayout([ MultiArrayDimension('rows', rows, rows*cols),
+ MultiArrayDimension('cols', cols, cols),
+ MultiArrayDimension('elem', 1, 1) ], 0 )
+ # Build the messages in a reasonable ROS format
+ intensity_msg = Float32MultiArrayStamped()
+ range_msg = Float32MultiArrayStamped()
+ info_msg = scans[-1].scan
+ joint_msg = Float32MultiArrayStamped()
+
+ intensity_msg.header = msg_header
+ range_msg.header = msg_header
+ joint_msg.header = msg_header
+
+ intensity_msg.data.layout = layout
+ range_msg.data.layout = layout
+ joint_msg.data.layout = MultiArrayLayout([ MultiArrayDimension('rows', rows, rows*2),
+ MultiArrayDimension('cols', 2, 2),
+ MultiArrayDimension('elem', 1, 1) ], 0 )
+ #joint_msg.data.layout = layout
+
+ # Clear out data from the info message. (Keep everything except intensity and range data)
+ #info_msg.ranges = [ ]
+ #info_msg.intensities = [ ]
+
+ # Populate intensities & ranges
+ intensity_msg.data.data = []
+ range_msg.data.data = []
+ for x in sorted_scans :
+ intensity_msg.data.data.extend(x.scan.intensities)
+ range_msg.data.data.extend(x.scan.ranges)
+ joint_msg.data.data.extend(x.pos)
+
+ intensity_pub.publish(intensity_msg)
+ range_pub.publish(range_msg)
+ info_pub.publish(info_msg)
+ joint_pos_pub.publish(joint_msg)
+
+ #image = sensor_msgs.msg.Image()
+
+ #image.header = msg_header
+ #image.label = 'intensity'
+ #image.encoding = 'mono'
+ #image.depth = 'uint8'
+ #max_val = max(joint_msg.data.data)
+ #min_val = min(joint_msg.data.data)
+ #image.uint8_data.data = "".join([chr(int(255*x/max_val)) for x in intensity_msg.data.data])
+ #image.uint8_data.data = "".join([chr(int(255*(x-min_val)/(max_val-min_val))) for x in joint_msg.data.data])
+ #image.uint8_data.data = ''
+ #image.uint8_data.layout = intensity_msg.data.layout
+ #image_pub.publish(image)
+
+
+ print ' Processed cloud with %u rays' % len(intensity_msg.data.data)
+
+if __name__ == '__main__':
+
+ req_lock = threading.Lock()
+
+ print 'Initializing DenseLaserCache'
+ laser_cache = DenseLaserCache(interval_req_callback, 200, 1000, 100) ;
+ print 'Done initializing'
+
+ rospy.init_node('dense_assembler', anonymous=False)
+ signal_sub = rospy.Subscriber("laser_tilt_controller/laser_scanner_signal",
+ LaserScannerSignal, signal_callback)
+
+ intensity_pub = rospy.Publisher("dense_tilt_scan/intensity", Float32MultiArrayStamped)
+ range_pub = rospy.Publisher("dense_tilt_scan/range", Float32MultiArrayStamped)
+ info_pub = rospy.Publisher("dense_tilt_scan/scan_info", LaserScan)
+ joint_pos_pub = rospy.Publisher("dense_tilt_scan/joint_info",Float32MultiArrayStamped)
+ #image_pub = rospy.Publisher("test_image", Image)
+
+ scan_sub = rospy.Subscriber("tilt_scan", LaserScan, scan_callback)
+
+ mech_sub = rospy.Subscriber("mechanism_state", MechanismState, mech_callback)
+
+ rospy.spin()
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/laser_image_node.py
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/laser_image_node.py (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/laser_image_node.py 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+
+# 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 Willow Garage, Inc. 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.
+#
+
+
+import roslib; roslib.load_manifest('dense_laser_assembler')
+import sys
+import rospy
+from time import sleep
+from laser_scan.msg import *
+from std_msgs.msg import *
+from dense_laser_assembler.msg import *
+from pr2_mechanism_controllers.msg import *
+from sensor_msgs.msg import Image
+
+
+# Takes the dense laser scan data and converts it into images to can be viewed in
+# image_view. Generally used as a debugging tool.
+
+
+def rescale(x, max_val) :
+ lower_lim = max_val*0.0
+ upper_lim = max_val*1.0
+ #lower_lim = 2000
+ #upper_lim = 3000
+ a = x - lower_lim
+ if a <= 0 :
+ return chr(0)
+ elif a > upper_lim -lower_lim :
+ return chr(255)
+
+ return chr(int((2**8-1)*a/(upper_lim-lower_lim)))
+
+def intensity_callback(msg) :
+ image = sensor_msgs.msg.Image()
+ image.header = msg.header
+ image.label = 'intensity'
+ image.encoding = 'mono'
+ image.depth = 'uint8'
+ max_val = max(msg.data.data)
+ #image.uint8_data.data = "".join([chr(int((2**8-1)*x/max_val)) for x in msg.data.data])
+ image.uint8_data.data = "".join([rescale(x,max_val) for x in msg.data.data])
+ image.uint8_data.layout = msg.data.layout
+ intensity_pub.publish(image)
+
+def range_callback(msg) :
+ image = sensor_msgs.msg.Image()
+ image.header = msg.header
+ image.label = 'range'
+ image.encoding = 'mono'
+ image.depth = 'uint8'
+ max_val = max(msg.data.data)
+ image.uint8_data.data = "".join([chr(int((2**8-1)*x/max_val)) for x in msg.data.data])
+ image.uint8_data.layout = msg.data.layout
+ range_pub.publish(image)
+
+if __name__ == '__main__':
+
+ rospy.init_node('laser_imager', anonymous=False)
+
+ intensity_pub = rospy.Publisher("dense_tilt_scan/intensity_image", Image)
+ range_pub = rospy.Publisher("dense_tilt_scan/range_image", Image)
+
+ intensity_sub = rospy.Subscriber("dense_tilt_scan/intensity", Float32MultiArrayStamped, intensity_callback)
+ range_sub = rospy.Subscriber("dense_tilt_scan/range", Float32MultiArrayStamped, range_callback)
+
+ rospy.spin()
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/src/build_dense_laser_snapshot.cpp
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/src/build_dense_laser_snapshot.cpp (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/src/build_dense_laser_snapshot.cpp 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,164 @@
+/*********************************************************************
+* 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 sensor_msgs::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.joint_velocities.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)
+ snapshot.joint_velocities.resize(num_joints*h*2) ;
+
+ // 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 & velocities
+ for (unsigned int j=0; j<num_joints; 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 ;
+ }
+
+ ROS_DEBUG("Done building snapshot that is [%u rows] x [%u cols]", 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 sensor_msgs::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/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/__init__.py
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/__init__.py (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/__init__.py 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1 @@
+
Added: pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py (rev 0)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py 2009-09-01 08:29:22 UTC (rev 23514)
@@ -0,0 +1,212 @@
+# 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 Willow Garage, Inc. 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.
+#
+
+import roslib
+roslib.load_manifest('dense_laser_assembler')
+
+import time
+import rospy
+import sys
+import threading
+from laser_scan.msg import *
+from roslib import rostime
+
+# Stores a single laser scan plus some meta information
+class LaserCacheElem() :
+ pass
+
+
+class DenseLaserCache() :
+ def __init__(self, req_callback, max_mech_len, max_laser_done_len, max_laser_wait_len) :
+
+ # Define callback for interval cloud req
+ self._req_callback = req_callback
+
+ # Specify all queue lengths
+ self._max_mech_len = max_mech_len
+ self._max_laser_done_len = max_laser_done_len
+ self._max_laser_wait_len = max_laser_wait_len
+
+ # Init mutexes
+ self._mech_lock = threading.Lock()
+ self._laser_done_lock = threading.Lock()
+ self._laser_wait_lock = threading.Lock()
+ self._interval_req_lock = threading.Lock()
+
+ # Define queues
+ self._mech_q = [ ] # Mechanism State Data
+ self._laser_done_q = [] # Laser scans that have already been processed
+ self._laser_wait_q = [] # Laser scans that we've received but haven't/can't processed yet
+ self._interval_req_q = [] # Stores list of intervals that we need to process
+
+ def add_mech_state(self, msg) :
+ self._mech_lock.acquire()
+ self._mech_q.append(msg)
+ while len(self._mec...
[truncated message content] |
