Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23655] pkg/trunk
|
From: <vij...@us...> - 2009-09-02 03:07:00
|
Revision: 23655
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23655&view=rev
Author: vijaypradeep
Date: 2009-09-02 03:06:51 +0000 (Wed, 02 Sep 2009)
Log Message:
-----------
Adding dense_laser_snapshotter. Compiles, but not tested. Still need to write unit-tests
Modified Paths:
--------------
pkg/trunk/stacks/calibration/dense_laser_assembler/include/dense_laser_assembler/dense_laser_assembler.h
pkg/trunk/stacks/calibration/dense_laser_assembler/manifest.xml
pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_assembler.cpp
Added Paths:
-----------
pkg/trunk/sandbox/pr2_dense_laser_snapshotter/
pkg/trunk/sandbox/pr2_dense_laser_snapshotter/CMakeLists.txt
pkg/trunk/sandbox/pr2_dense_laser_snapshotter/Makefile
pkg/trunk/sandbox/pr2_dense_laser_snapshotter/mainpage.dox
pkg/trunk/sandbox/pr2_dense_laser_snapshotter/manifest.xml
pkg/trunk/sandbox/pr2_dense_laser_snapshotter/src/
pkg/trunk/sandbox/pr2_dense_laser_snapshotter/src/dense_laser_snapshotter.cpp
Removed Paths:
-------------
pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_snapshotter.cpp
Added: pkg/trunk/sandbox/pr2_dense_laser_snapshotter/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/pr2_dense_laser_snapshotter/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/pr2_dense_laser_snapshotter/CMakeLists.txt 2009-09-02 03:06:51 UTC (rev 23655)
@@ -0,0 +1,30 @@
+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)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+# rosbuild_genmsg()
+# rosbuild_gensrv()
+
+rosbuild_add_executable(dense_laser_snapshotter src/dense_laser_snapshotter.cpp)
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Added: pkg/trunk/sandbox/pr2_dense_laser_snapshotter/Makefile
===================================================================
--- pkg/trunk/sandbox/pr2_dense_laser_snapshotter/Makefile (rev 0)
+++ pkg/trunk/sandbox/pr2_dense_laser_snapshotter/Makefile 2009-09-02 03:06:51 UTC (rev 23655)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/sandbox/pr2_dense_laser_snapshotter/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/pr2_dense_laser_snapshotter/mainpage.dox (rev 0)
+++ pkg/trunk/sandbox/pr2_dense_laser_snapshotter/mainpage.dox 2009-09-02 03:06:51 UTC (rev 23655)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b pr2_dense_laser_snapshotter is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/sandbox/pr2_dense_laser_snapshotter/manifest.xml
===================================================================
--- pkg/trunk/sandbox/pr2_dense_laser_snapshotter/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/pr2_dense_laser_snapshotter/manifest.xml 2009-09-02 03:06:51 UTC (rev 23655)
@@ -0,0 +1,18 @@
+<package>
+ <description brief="pr2_dense_laser_snapshotter">
+
+ pr2_dense_laser_snapshotter
+
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/pr2_dense_laser_snapshotter</url>
+
+ <depend package="dense_laser_assembler"/>
+ <depend package="pr2_msgs"/>
+
+
+</package>
+
+
Copied: pkg/trunk/sandbox/pr2_dense_laser_snapshotter/src/dense_laser_snapshotter.cpp (from rev 23654, pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_snapshotter.cpp)
===================================================================
--- pkg/trunk/sandbox/pr2_dense_laser_snapshotter/src/dense_laser_snapshotter.cpp (rev 0)
+++ pkg/trunk/sandbox/pr2_dense_laser_snapshotter/src/dense_laser_snapshotter.cpp 2009-09-02 03:06:51 UTC (rev 23655)
@@ -0,0 +1,120 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "ros/ros.h"
+
+#include <dense_laser_assembler/dense_laser_assembler.h>
+
+// Messages
+#include "sensor_msgs/LaserScan.h"
+#include "calibration_msgs/DenseLaserSnapshot.h"
+#include "pr2_msgs/LaserScannerSignal.h"
+
+using namespace dense_laser_assembler ;
+
+/**
+ * \brief Builds a DenseLaserSnapshot message from laser scans collected in a specified time interval
+ */
+class DenseLaserSnapshotter
+{
+
+public:
+
+ DenseLaserSnapshotter()
+ {
+ prev_signal_.header.stamp = ros::Time(0, 0);
+ signal_sub_ = n_.subscribe("laser_tilt_controller/laser_scanner_signal", 2, &DenseLaserSnapshotter::scannerSignalCallback, this);
+ snapshot_pub_ = n_.advertise<calibration_msgs::DenseLaserSnapshot> ("dense_laser_snapshot", 1);
+ first_time_ = true;
+ }
+
+ ~DenseLaserSnapshotter()
+ {
+
+ }
+
+ void scannerSignalCallback(const pr2_msgs::LaserScannerSignalConstPtr& cur_signal)
+ {
+ ROS_DEBUG("Got Scanner Signal") ;
+
+ if (first_time_)
+ {
+ prev_signal_ = *cur_signal ;
+ first_time_ = false ;
+ }
+ else
+ {
+ if (cur_signal->signal == 1)
+ {
+ ROS_DEBUG("About to make request");
+
+ ros::Time start = prev_signal_.header.stamp;
+ ros::Time end = cur_signal->header.stamp;
+
+ calibration_msgs::DenseLaserSnapshot snapshot;
+
+ assembler_.assembleSnapshot(start, end, snapshot);
+
+ ROS_DEBUG("header.stamp: %f", snapshot.header.stamp.toSec());
+ ROS_DEBUG("header.frame_id: %s", snapshot.header.frame_id.c_str());
+ ROS_DEBUG("ranges.size()=%u", snapshot.ranges.size());
+ ROS_DEBUG("intensities.size()=%u", snapshot.intensities.size());
+ ROS_DEBUG("scan_start.size()=%u", snapshot.scan_start.size());
+ snapshot_pub_.publish(snapshot);
+ }
+ else
+ ROS_DEBUG("Not making request");
+ prev_signal_ = *cur_signal;
+ }
+ }
+
+private:
+ ros::NodeHandle n_;
+ ros::Publisher snapshot_pub_;
+ ros::Subscriber signal_sub_;
+ ros::Subscriber scan_sub_;
+ DenseLaserAssembler assembler_;
+ pr2_msgs::LaserScannerSignal prev_signal_;
+
+ bool first_time_ ;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "dense_laser_snapshotter") ;
+ DenseLaserSnapshotter snapshotter ;
+ ros::spin() ;
+
+ return 0;
+}
Modified: pkg/trunk/stacks/calibration/dense_laser_assembler/include/dense_laser_assembler/dense_laser_assembler.h
===================================================================
--- pkg/trunk/stacks/calibration/dense_laser_assembler/include/dense_laser_assembler/dense_laser_assembler.h 2009-09-02 03:02:30 UTC (rev 23654)
+++ pkg/trunk/stacks/calibration/dense_laser_assembler/include/dense_laser_assembler/dense_laser_assembler.h 2009-09-02 03:06:51 UTC (rev 23655)
@@ -60,8 +60,9 @@
* \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 on success
*/
- void assembleSnapshot(const ros::Time& start, const ros::Time& end, calibration_msgs::DenseLaserSnapshot& snapshot);
+ bool assembleSnapshot(const ros::Time& start, const ros::Time& end, calibration_msgs::DenseLaserSnapshot& snapshot);
private:
Modified: pkg/trunk/stacks/calibration/dense_laser_assembler/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/dense_laser_assembler/manifest.xml 2009-09-02 03:02:30 UTC (rev 23654)
+++ pkg/trunk/stacks/calibration/dense_laser_assembler/manifest.xml 2009-09-02 03:06:51 UTC (rev 23655)
@@ -22,7 +22,7 @@
<depend package="settlerlib" />
<export>
- <cpp cflags="-I{prefix}/include" />
+ <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -ldense_laser_assembler"/>
</export>
</package>
Modified: pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_assembler.cpp
===================================================================
--- pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_assembler.cpp 2009-09-02 03:02:30 UTC (rev 23654)
+++ pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_assembler.cpp 2009-09-02 03:06:51 UTC (rev 23655)
@@ -53,8 +53,15 @@
cache_.add(laser_scan);
}
+bool DenseLaserAssembler::assembleSnapshot(const ros::Time& start, const ros::Time& end, calibration_msgs::DenseLaserSnapshot& snapshot)
+{
+ vector< sensor_msgs::LaserScanConstPtr > scan_vec;
+ cache_.getInterval(start, end);
+ return flattenScanVec(scan_vec, snapshot);
+}
+
bool DenseLaserAssembler::flattenScanVec(const std::vector< sensor_msgs::LaserScanConstPtr >& scans,
- calibration_msgs::DenseLaserSnapshot& snapshot)
+ calibration_msgs::DenseLaserSnapshot& snapshot)
{
if (scans.size() == 0)
{
Deleted: pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_snapshotter.cpp
===================================================================
--- pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_snapshotter.cpp 2009-09-02 03:02:30 UTC (rev 23654)
+++ pkg/trunk/stacks/calibration/dense_laser_assembler/src/dense_laser_snapshotter.cpp 2009-09-02 03:06:51 UTC (rev 23655)
@@ -1,132 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include "ros/ros.h"
-
-
-#include "message_filters/subscriber.h"
-#include "dense_laser_assembler/dense_laser_msg_filter.h"
-
-// Messages
-#include "calibration_msgs/DenseLaserSnapshot.h"
-#include "pr2_msgs/LaserScannerSignal.h"
-
-
-using namespace dense_laser_assembler ;
-
-/**
- * \brief Builds a DenseLaserSnapshot message from laser scans collected in a specified time interval
- */
-class DenseLaserSnapshotter
-{
-
-public:
-
- DenseLaserSnapshotter() :
- scan_sub_(n_, "tilt_scan", 20),
- mech_state_sub_(n_, "mechanism_state", 20),
- dense_laser_filter_("dense_laser_filter", scan_sub_, mech_state_sub_)
- {
- prev_signal_.header.stamp.fromNSec(0) ;
- signal_sub_ = n_.subscribe("laser_tilt_controller/laser_scanner_signal", 2, &DenseLaserSnapshotter::scannerSignalCallback, this) ;
- snapshot_pub_ = n_.advertise<calibration_msgs::DenseLaserSnapshot> ("dense_laser_snapshot", 1) ;
- first_time_ = true ;
- }
-
- ~DenseLaserSnapshotter()
- {
-
- }
-
- void scannerSignalCallback(const pr2_msgs::LaserScannerSignalConstPtr& cur_signal)
- {
- ROS_DEBUG("Got Scanner Signal") ;
- if (cur_signal->signal == 128 || cur_signal->signal == 129) // These codes imply that this is the first signal during a given profile type
- first_time_ = true ;
-
-
- if (first_time_)
- {
- prev_signal_ = *cur_signal ;
- first_time_ = false ;
- }
- else
- {
- if (cur_signal->signal == 1)
- {
- ROS_DEBUG("About to make request") ;
-
- ros::Time start = prev_signal_.header.stamp ;
- ros::Time end = cur_signal->header.stamp ;
-
- calibration_msgs::DenseLaserSnapshot snapshot ;
-
- dense_laser_filter_.buildSnapshotFromInterval(start, end, snapshot) ;
-
- ROS_DEBUG("header.stamp: %f", snapshot.header.stamp.toSec()) ;
- ROS_DEBUG("header.frame_id: %s", snapshot.header.frame_id.c_str()) ;
- ROS_DEBUG("ranges.size()=%u", snapshot.ranges.size()) ;
- ROS_DEBUG("intensities.size()=%u", snapshot.intensities.size()) ;
- ROS_DEBUG("joint_positions.size()=%u", snapshot.joint_positions.size()) ;
- ROS_DEBUG("joint_velocities.size()=%u", snapshot.joint_velocities.size()) ;
- ROS_DEBUG("scan_start.size()=%u", snapshot.scan_start.size()) ;
- snapshot_pub_.publish(snapshot) ;
-
- }
- else
- ROS_DEBUG("Not making request") ;
- prev_signal_ = *cur_signal ;
- }
- }
-
-private:
- ros::NodeHandle n_ ;
- ros::Publisher snapshot_pub_ ;
- ros::Subscriber signal_sub_ ;
- message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_ ;
- message_filters::Subscriber<pr2_mechanism_msgs::MechanismState> mech_state_sub_ ;
- DenseLaserMsgFilter dense_laser_filter_ ;
- pr2_msgs::LaserScannerSignal prev_signal_;
-
- bool first_time_ ;
-} ;
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "dense_laser_snapshotter") ;
- DenseLaserSnapshotter snapshotter ;
- ros::spin() ;
-
- return 0;
-}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
