Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23610] pkg/trunk
|
From: <vij...@us...> - 2009-09-01 23:30:43
|
Revision: 23610
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23610&view=rev
Author: vijaypradeep
Date: 2009-09-01 23:30:28 +0000 (Tue, 01 Sep 2009)
Log Message:
-----------
Moving dense_laser_assembler into calibration stack
Added Paths:
-----------
pkg/trunk/stacks/dense_laser_assembler/
pkg/trunk/stacks/dense_laser_assembler/CMakeLists.txt
pkg/trunk/stacks/dense_laser_assembler/Makefile
pkg/trunk/stacks/dense_laser_assembler/ROS_BUILD_BLACKLIST
pkg/trunk/stacks/dense_laser_assembler/include/
pkg/trunk/stacks/dense_laser_assembler/include/dense_laser_assembler/
pkg/trunk/stacks/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h
pkg/trunk/stacks/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
pkg/trunk/stacks/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
pkg/trunk/stacks/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
pkg/trunk/stacks/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h
pkg/trunk/stacks/dense_laser_assembler/mainpage.dox
pkg/trunk/stacks/dense_laser_assembler/manifest.xml
pkg/trunk/stacks/dense_laser_assembler/msg/
pkg/trunk/stacks/dense_laser_assembler/msg/Float32MultiArrayStamped.msg
pkg/trunk/stacks/dense_laser_assembler/msg/JointPVArray.msg
pkg/trunk/stacks/dense_laser_assembler/scripts/
pkg/trunk/stacks/dense_laser_assembler/scripts/assembler_node.py
pkg/trunk/stacks/dense_laser_assembler/scripts/laser_image_node.py
pkg/trunk/stacks/dense_laser_assembler/src/
pkg/trunk/stacks/dense_laser_assembler/src/build_dense_laser_snapshot.cpp
pkg/trunk/stacks/dense_laser_assembler/src/dense_laser_assembler/
pkg/trunk/stacks/dense_laser_assembler/src/dense_laser_assembler/__init__.py
pkg/trunk/stacks/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
pkg/trunk/stacks/dense_laser_assembler/src/dense_laser_assembler_srv.cpp
pkg/trunk/stacks/dense_laser_assembler/src/dense_laser_msg_filter.cpp
pkg/trunk/stacks/dense_laser_assembler/src/dense_laser_snapshotter.cpp
pkg/trunk/stacks/dense_laser_assembler/src/joint_extractor_display.cpp
pkg/trunk/stacks/dense_laser_assembler/src/laser_imager.cpp
pkg/trunk/stacks/dense_laser_assembler/src/tagged_laser_cache_display.cpp
pkg/trunk/stacks/dense_laser_assembler/srv/
pkg/trunk/stacks/dense_laser_assembler/srv/BuildLaserSnapshot.srv
Removed 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
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/CMakeLists.txt 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/CMakeLists.txt 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,35 +0,0 @@
-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()
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/Makefile
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/Makefile 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/Makefile 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/ROS_BUILD_BLACKLIST 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/ROS_BUILD_BLACKLIST 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1 +0,0 @@
-
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/build_dense_laser_snapshot.h 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,76 +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_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
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,201 +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.
-*********************************************************************/
-
-//! \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
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,155 +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_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_
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,217 +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_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_ */
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/include/dense_laser_assembler/tagged_laser_scan.h 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,54 +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.
-*********************************************************************/
-
-#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_
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/mainpage.dox
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/mainpage.dox 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/mainpage.dox 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,28 +0,0 @@
-/**
-\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
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/manifest.xml
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/manifest.xml 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/manifest.xml 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,33 +0,0 @@
-<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>
-
-
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/msg/Float32MultiArrayStamped.msg
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/msg/Float32MultiArrayStamped.msg 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/msg/Float32MultiArrayStamped.msg 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,2 +0,0 @@
-Header header
-std_msgs/Float32MultiArray data
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/msg/JointPVArray.msg
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/msg/JointPVArray.msg 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/msg/JointPVArray.msg 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,4 +0,0 @@
-# Stores a vector of positions
-Header header
-float64[] pos
-float64[] vel
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/assembler_node.py
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/assembler_node.py 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/assembler_node.py 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,188 +0,0 @@
-#!/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()
Deleted: pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/laser_image_node.py
===================================================================
--- pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/laser_image_node.py 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/scripts/laser_image_node.py 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,98 +0,0 @@
-#!/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()
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/src/build_dense_laser_snapshot.cpp 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,164 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include "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 ;
-}
-
-}
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/__init__.py 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1 +0,0 @@
-
Deleted: 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 2009-09-01 23:03:52 UTC (rev 23609)
+++ pkg/trunk/calibration_experimental/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py 2009-09-01 23:30:28 UTC (rev 23610)
@@ -1,212 +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 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...
[truncated message content] |
