Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23640] pkg/trunk
|
From: <vij...@us...> - 2009-09-02 02:18:40
|
Revision: 23640
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23640&view=rev
Author: vijaypradeep
Date: 2009-09-02 02:18:28 +0000 (Wed, 02 Sep 2009)
Log Message:
-----------
Moving pr2_laser_snapshotter into pr2 stack
Added Paths:
-----------
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/CMakeLists.txt
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/Makefile
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/mainpage.dox
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/manifest.xml
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/srv/
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_assembler.cpp
pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch
Removed Paths:
-------------
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
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/CMakeLists.txt
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/CMakeLists.txt 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/CMakeLists.txt 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,40 +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)
-
-rosbuild_init()
-
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-# rosbuild_genmsg()
-rosbuild_gensrv()
-
-rosbuild_add_executable(pr2_laser_snapshotter src/pr2_laser_snapshotter.cpp)
-rosbuild_add_executable(point_cloud_srv src/point_cloud_srv.cpp)
-
-# Needed to run rostests
-rosbuild_add_executable(dummy_scan_producer test/dummy_scan_producer.cpp)
-
-rosbuild_add_executable(test_assembler
- test/test_assembler.cpp)
-rosbuild_link_boost(test_assembler thread)
-rosbuild_add_gtest_build_flags(test_assembler)
-
-rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_laser_assembler.launch)
-
-#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})
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/Makefile
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/Makefile 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/Makefile 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/mainpage.dox
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/mainpage.dox 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/mainpage.dox 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,119 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b pr2_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
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/manifest.xml
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/manifest.xml 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/manifest.xml 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,30 +0,0 @@
-<package>
- <description brief="Build point clouds from PR2's tilting laser mechanism">
- Uses the laser_assembler to generate points clouds from each sweep from PR2's
- tilting laser mechanism.
- </description>
- <author>Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/pr2_laser_snapshotter</url>
-
- <export>
- <cpp cflags="-I${prefix}/srv/cpp" />
- </export>
-
- <!-- ROS -->
- <depend package="roscpp" />
-
- <!-- common_msgs -->
- <depend package="sensor_msgs" />
- <depend package="geometry_msgs" />
-
- <!-- pr2_msgs -->
- <depend package="pr2_msgs" />
-
- <!-- laser_pipeline -->
- <depend package="laser_assembler" />
-
-</package>
-
-
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,156 +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/node.h"
-
-// Services
-#include "laser_assembler/AssembleScans.h"
-#include "pr2_laser_snapshotter/BuildCloudAngle.h"
-#include "pr2_msgs/SetPeriodicCmd.h"
-
-// Messages
-#include "sensor_msgs/PointCloud.h"
-#include "pr2_msgs/LaserScannerSignal.h"
-
-#include <boost/thread/mutex.hpp>
-
-
-/***
- * This uses the point_cloud_assembler's build_cloud service call to grab all the scans/clouds between two tilt-laser shutters
- * params
- * * "~target_frame_id" (string) - This is the frame that the scanned data transformed into. The
- * output clouds are also published in this frame.
- */
-
-namespace pr2_laser_snapshotter
-{
-
-using namespace ros;
-using namespace std;
-
-class PointCloudSrv : public ros::Node
-{
-private:
- pr2_msgs::LaserScannerSignal laser_scanner_signal_;
- ros::Time laser_time_;
- boost::mutex laser_mutex_;
-
-
-public:
- PointCloudSrv() :
- ros::Node("point_cloud_srv")
- {
- advertiseService("point_cloud_srv/single_sweep_cloud", &PointCloudSrv::buildSingleSweepCloud, this);
- subscribe("laser_tilt_controller/laser_scanner_signal", laser_scanner_signal_, &PointCloudSrv::scannerSignalCallback, 40) ;
-
- laser_time_ = Time().fromSec(0);
- }
-
- ~PointCloudSrv()
- {
- unsubscribe("laser_tilt_controller/laser_scanner_signal") ;
- unadvertiseService("point_cloud_srv/single_sweep_cloud") ;
- }
-
-
- bool buildSingleSweepCloud(pr2_laser_snapshotter::BuildCloudAngle::Request &req,
- pr2_laser_snapshotter::BuildCloudAngle::Response &res)
- {
- // send command to tilt laser scanner
- pr2_msgs::SetPeriodicCmd::Request scan_req;
- pr2_msgs::SetPeriodicCmd::Response scan_res;
- scan_req.command.amplitude = fabs(req.angle_end - req.angle_begin)/2.0;
- scan_req.command.offset = (req.angle_end + req.angle_begin)/2.0;
- scan_req.command.period = req.duration*2.0;
- scan_req.command.profile = "linear";
- if (!ros::service::call("laser_tilt_controller/set_periodic_cmd", scan_req, scan_res))
- ROS_ERROR("PointCloudSrv: error setting laser scanner periodic command");
- else
- ROS_INFO("PointCloudSrv: commanded tilt laser scanner with period %f, amplitude %f and offset %f",
- scan_req.command.period, scan_req.command.amplitude, scan_req.command.offset);
-
-
- // wait for signal from laser to know when scan is finished
- Time begin_time = scan_res.start_time;
- Duration timeout = Duration().fromSec(2.0);
- while (laser_time_ < begin_time){
- boost::mutex::scoped_lock laser_lock(laser_mutex_);
- if (ros::Time::now() > begin_time + Duration().fromSec(req.duration) + timeout){
- ROS_ERROR("PointCloudSrv: Timeout waiting for laser scan to come in");
- return false;
- }
- laser_lock.unlock();
- Duration().fromSec(0.05).sleep();
- }
- Time end_time = laser_time_;
- ROS_INFO("PointCloudSrv: generated point cloud from time %f to %f", begin_time.toSec(), end_time.toSec());
-
- // get a point cloud from the point cloud assembler
- laser_assembler::AssembleScans::Request assembler_req ;
- laser_assembler::AssembleScans::Response assembler_res ;
- assembler_req.begin = begin_time;
- assembler_req.end = end_time;
- if (!ros::service::call("laser_scan_assembler/build_cloud", assembler_req, assembler_res))
- ROS_ERROR("PointCloudSrv: error receiving point cloud from point cloud assembler");
- else
- ROS_INFO("PointCloudSrv: received point cloud of size %i from point cloud assembler", assembler_res.cloud.points.size());
-
- res.cloud = assembler_res.cloud;
- return true;
- }
-
-
- void scannerSignalCallback()
- {
- boost::mutex::scoped_lock laser_lock(laser_mutex_);
- // note time when tilt laser is pointing down
- if (laser_scanner_signal_.signal == 1){
- laser_time_ = laser_scanner_signal_.header.stamp;
- }
-}
-
-} ;
-
-}
-
-
-using namespace pr2_laser_snapshotter;
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv);
- PointCloudSrv cloud_srv;
- cloud_srv.spin();
-
- return 0;
-}
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,145 +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/node.h"
-
-// Services
-#include "laser_assembler/AssembleScans.h"
-
-// Messages
-#include "sensor_msgs/PointCloud.h"
-#include "pr2_msgs/LaserScannerSignal.h"
-
-/***
- * This uses the point_cloud_assembler's build_cloud service call to grab all the scans/clouds between two tilt-laser shutters
- * params
- * * "~target_frame_id" (string) - This is the frame that the scanned data transformed into. The
- * output clouds are also published in this frame.
- * * "~num_skips" (int) - If set to N>0, then the snapshotter will skip N signals before
- * requesting a new snapshot. This will make the snapshots be N times
- * larger. Default 0 - no skipping.
- */
-
-namespace pr2_laser_snapshotter
-{
-
-class PR2LaserSnapshotter
-{
-
-public:
- ros::NodeHandle n_;
- ros::Publisher pub_;
- ros::Subscriber sub_;
-
- pr2_msgs::LaserScannerSignal prev_signal_;
-
- bool first_time_;
-
- int num_skips_;
- int num_skips_left_;
-
- std::string fixed_frame_;
-
- PR2LaserSnapshotter()
- {
- prev_signal_.header.stamp.fromNSec(0);
-
- pub_ = n_.advertise<sensor_msgs::PointCloud> ("full_cloud", 1);
- sub_ = n_.subscribe("laser_scanner_signal", 40, &PR2LaserSnapshotter::scannerSignalCallback, this);
-
- n_.param("~num_skips", num_skips_, 0);
- num_skips_left_=num_skips_;
-
- first_time_ = true;
- }
-
- ~PR2LaserSnapshotter()
- {
-
- }
-
- void scannerSignalCallback(const pr2_msgs::LaserScannerSignalConstPtr& cur_signal)
- {
- if (cur_signal->signal == 128 || cur_signal->signal == 129) // These codes imply that this is the first signal during a given profile type
- first_time_ = true;
-
-
- if (first_time_)
- {
- prev_signal_ = *cur_signal;
- first_time_ = false;
- }
- else
- {
- if(num_skips_>0)
- {
- if(num_skips_left_>0)
- {
- num_skips_left_ -= 1;
- return;
- }
- else
- {
- num_skips_left_=num_skips_;
- }
- }
-
- laser_assembler::AssembleScans::Request req;
- laser_assembler::AssembleScans::Response resp;
-
- req.begin = prev_signal_.header.stamp;
- req.end = cur_signal->header.stamp;
-
- if (!ros::service::call("build_cloud", req, resp))
- ROS_ERROR("Failed to call service on point cloud assembler or laser scan assembler.");
-
- pub_.publish(resp.cloud);
- ROS_DEBUG("Snapshotter::Published Cloud size=%u", resp.cloud.get_points_size());
-
- prev_signal_ = *cur_signal;
- }
- }
-} ;
-
-}
-
-using namespace pr2_laser_snapshotter;
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "pr2_laser_snapshotter");
- PR2LaserSnapshotter snapshotter ;
- ros::spin();
- return 0;
-}
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,5 +0,0 @@
-float64 angle_begin
-float64 angle_end
-float64 duration
----
-sensor_msgs/PointCloud cloud
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,121 +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 */
-
-/**
- * Generate dummy scans in order to not be dependent on bag files in order to run tests
- **/
-
-#include <boost/thread.hpp>
-#include <ros/ros.h>
-#include <tf/transform_broadcaster.h>
-#include <sensor_msgs/LaserScan.h>
-#include <pr2_msgs/LaserScannerSignal.h>
-
-
-void runLoop()
-{
- ros::NodeHandle nh;
-
- ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("dummy_scan", 100);
- ros::Publisher signal_pub = nh.advertise<pr2_msgs::LaserScannerSignal>("dummy_signal", 100);
- ros::Rate loop_rate(20);
-
- // Configure the Transform broadcaster
- tf::TransformBroadcaster broadcaster;
- tf::Transform laser_transform(btQuaternion(0,0,0,1));
-
- // Populate the dummy laser scan
- sensor_msgs::LaserScan scan;
- scan.header.frame_id = "dummy_laser_link";
- scan.angle_min = 0.0;
- scan.angle_max = 99.0;
- scan.angle_increment = 1.0;
- scan.time_increment = .001;
- scan.scan_time = .05;
- scan.range_min = .01;
- scan.range_max = 100.0;
-
- const unsigned int N = 100;
- scan.ranges.resize(N);
- scan.intensities.resize(N);
-
- for (unsigned int i=0; i<N; i++)
- {
- scan.ranges[i] = 10.0;
- scan.intensities[i] = 10.0;
- }
-
- unsigned int cloud_count = 0;
-
- // Immeadiately send out a starting scan
- pr2_msgs::LaserScannerSignal signal;
- signal.header.stamp = scan.header.stamp;
- signal.signal = 1;
- signal_pub.publish(signal);
-
- // Loop for each cloud
- //while (nh.ok() & cloud_count < 1000)
- while (nh.ok())
- {
- unsigned int scan_count = 0;
- // Loop for each scan
- while (nh.ok() & scan_count < 10)
- {
- scan.header.stamp = ros::Time::now();
- scan_pub.publish(scan);
- broadcaster.sendTransform(laser_transform, scan.header.stamp, "dummy_laser_link", "dummy_base_link");
- scan_count++;
- loop_rate.sleep();
- }
-
- signal.header.stamp = scan.header.stamp;
- signal.signal = cloud_count % 2;
- signal_pub.publish(signal);
-
- ROS_INFO("Done publishing cloud [%u]", cloud_count);
- cloud_count++;
- }
-}
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "scan_producer");
- ros::NodeHandle nh;
- boost::thread run_thread(&runLoop);
- ros::spin();
- run_thread.join();
- return 0;
-}
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/test/test_assembler.cpp
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/test/test_assembler.cpp 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/test/test_assembler.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,140 +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 */
-
-#include <string>
-#include <gtest/gtest.h>
-#include "ros/node.h"
-#include "sensor_msgs/PointCloud.h"
-#include "sensor_msgs/LaserScan.h"
-#include "boost/thread.hpp"
-
-using namespace ros;
-using namespace sensor_msgs;
-
-int g_argc;
-char** g_argv;
-
-class TestAssembler : public testing::Test
-{
-public:
-
- NodeHandle n_;
-
- sensor_msgs::PointCloud cloud_msg_ ;
- boost::mutex cloud_mutex_ ;
- sensor_msgs::PointCloud safe_cloud_ ;
- int cloud_counter_ ;
-
- LaserScan scan_msg_ ;
- boost::mutex scan_mutex_ ;
- LaserScan safe_scan_ ;
- int scan_counter_ ;
-
-
- void CloudCallback(const PointCloudConstPtr& cloud_msg)
- {
- cloud_mutex_.lock() ;
- cloud_counter_++ ;
- safe_cloud_ = *cloud_msg ;
- cloud_mutex_.unlock() ;
- ROS_INFO("Got Cloud with %u points", cloud_msg_.get_points_size()) ;
- }
-
- void ScanCallback(const LaserScanConstPtr& scan_msg)
- {
- scan_mutex_.lock() ;
- scan_counter_++ ;
- safe_scan_ = *scan_msg ;
- scan_mutex_.unlock() ;
- //ROS_INFO("Got Scan") ;
- }
-
-protected:
- /// constructor
- TestAssembler()
- {
- cloud_counter_ = 0 ;
- scan_counter_ = 0 ;
- }
-
- /// Destructor
- ~TestAssembler()
- {
-
- }
-};
-
-
-TEST_F(TestAssembler, test)
-{
- ROS_INFO("Starting test F");
- ros::Subscriber cloud_sub = n_.subscribe("dummy_cloud", 10, &TestAssembler::CloudCallback, (TestAssembler*)this);
- ros::Subscriber scan_sub = n_.subscribe("dummy_scan", 10, &TestAssembler::ScanCallback, (TestAssembler*)this);
-
- bool waiting = true;
- while(n_.ok() && waiting )
- {
- ros::spinOnce();
- usleep(1e2) ;
-
- scan_mutex_.lock() ;
- waiting = (scan_counter_ < 55) ;
- scan_mutex_.unlock() ;
- }
-
- usleep(1e6) ;
-
- ASSERT_GT(cloud_counter_, 0) ;
-
- unsigned int cloud_size ;
- cloud_mutex_.lock() ;
- cloud_size = safe_cloud_.get_points_size() ;
- cloud_mutex_.unlock() ;
-
- EXPECT_EQ((unsigned int) 1000, cloud_size) ;
-
- SUCCEED();
-}
-
-int main(int argc, char** argv)
-{
- printf("******* Starting application *********\n");
-
- testing::InitGoogleTest(&argc, argv);
- ros::init(argc, argv, "test_assembler_node");
-
- return RUN_ALL_TESTS();
-}
Deleted: pkg/trunk/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch
===================================================================
--- pkg/trunk/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch 2009-09-02 02:14:28 UTC (rev 23639)
+++ pkg/trunk/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch 2009-09-02 02:18:28 UTC (rev 23640)
@@ -1,21 +0,0 @@
-<launch>
-
- <node pkg="laser_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
- <remap from="scan_in" to="dummy_scan"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="dummy_base_link" />
- </node>
-
- <node pkg="pr2_laser_snapshotter" type="pr2_laser_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="dummy_signal"/>
- <remap from="build_cloud" to="laser_scan_assembler/build_cloud" />
- <remap from="full_cloud" to="dummy_cloud" />
- </node>
-
- <node pkg="pr2_laser_snapshotter" type="dummy_scan_producer" output="screen"/>
-
- <test test-name="test_laser_assembler" pkg="pr2_laser_snapshotter" type="test_assembler" />
-
-</launch>
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/CMakeLists.txt (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/CMakeLists.txt)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/CMakeLists.txt 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,40 @@
+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(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+# rosbuild_genmsg()
+rosbuild_gensrv()
+
+rosbuild_add_executable(pr2_laser_snapshotter src/pr2_laser_snapshotter.cpp)
+rosbuild_add_executable(point_cloud_srv src/point_cloud_srv.cpp)
+
+# Needed to run rostests
+rosbuild_add_executable(dummy_scan_producer test/dummy_scan_producer.cpp)
+
+rosbuild_add_executable(test_assembler
+ test/test_assembler.cpp)
+rosbuild_link_boost(test_assembler thread)
+rosbuild_add_gtest_build_flags(test_assembler)
+
+rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_laser_assembler.launch)
+
+#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})
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/Makefile (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/Makefile)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/Makefile (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/Makefile 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/mainpage.dox (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/mainpage.dox)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/mainpage.dox (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/mainpage.dox 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b pr2_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
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/manifest.xml (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/manifest.xml)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/manifest.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/manifest.xml 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,30 @@
+<package>
+ <description brief="Build point clouds from PR2's tilting laser mechanism">
+ Uses the laser_assembler to generate points clouds from each sweep from PR2's
+ tilting laser mechanism.
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/pr2_laser_snapshotter</url>
+
+ <export>
+ <cpp cflags="-I${prefix}/srv/cpp" />
+ </export>
+
+ <!-- ROS -->
+ <depend package="roscpp" />
+
+ <!-- common_msgs -->
+ <depend package="sensor_msgs" />
+ <depend package="geometry_msgs" />
+
+ <!-- pr2_msgs -->
+ <depend package="pr2_msgs" />
+
+ <!-- laser_pipeline -->
+ <depend package="laser_assembler" />
+
+</package>
+
+
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,156 @@
+/********************************************************************* * 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/node.h"
+
+// Services
+#include "laser_assembler/AssembleScans.h"
+#include "pr2_laser_snapshotter/BuildCloudAngle.h"
+#include "pr2_msgs/SetPeriodicCmd.h"
+
+// Messages
+#include "sensor_msgs/PointCloud.h"
+#include "pr2_msgs/LaserScannerSignal.h"
+
+#include <boost/thread/mutex.hpp>
+
+
+/***
+ * This uses the point_cloud_assembler's build_cloud service call to grab all the scans/clouds between two tilt-laser shutters
+ * params
+ * * "~target_frame_id" (string) - This is the frame that the scanned data transformed into. The
+ * output clouds are also published in this frame.
+ */
+
+namespace pr2_laser_snapshotter
+{
+
+using namespace ros;
+using namespace std;
+
+class PointCloudSrv : public ros::Node
+{
+private:
+ pr2_msgs::LaserScannerSignal laser_scanner_signal_;
+ ros::Time laser_time_;
+ boost::mutex laser_mutex_;
+
+
+public:
+ PointCloudSrv() :
+ ros::Node("point_cloud_srv")
+ {
+ advertiseService("point_cloud_srv/single_sweep_cloud", &PointCloudSrv::buildSingleSweepCloud, this);
+ subscribe("laser_tilt_controller/laser_scanner_signal", laser_scanner_signal_, &PointCloudSrv::scannerSignalCallback, 40) ;
+
+ laser_time_ = Time().fromSec(0);
+ }
+
+ ~PointCloudSrv()
+ {
+ unsubscribe("laser_tilt_controller/laser_scanner_signal") ;
+ unadvertiseService("point_cloud_srv/single_sweep_cloud") ;
+ }
+
+
+ bool buildSingleSweepCloud(pr2_laser_snapshotter::BuildCloudAngle::Request &req,
+ pr2_laser_snapshotter::BuildCloudAngle::Response &res)
+ {
+ // send command to tilt laser scanner
+ pr2_msgs::SetPeriodicCmd::Request scan_req;
+ pr2_msgs::SetPeriodicCmd::Response scan_res;
+ scan_req.command.amplitude = fabs(req.angle_end - req.angle_begin)/2.0;
+ scan_req.command.offset = (req.angle_end + req.angle_begin)/2.0;
+ scan_req.command.period = req.duration*2.0;
+ scan_req.command.profile = "linear";
+ if (!ros::service::call("laser_tilt_controller/set_periodic_cmd", scan_req, scan_res))
+ ROS_ERROR("PointCloudSrv: error setting laser scanner periodic command");
+ else
+ ROS_INFO("PointCloudSrv: commanded tilt laser scanner with period %f, amplitude %f and offset %f",
+ scan_req.command.period, scan_req.command.amplitude, scan_req.command.offset);
+
+
+ // wait for signal from laser to know when scan is finished
+ Time begin_time = scan_res.start_time;
+ Duration timeout = Duration().fromSec(2.0);
+ while (laser_time_ < begin_time){
+ boost::mutex::scoped_lock laser_lock(laser_mutex_);
+ if (ros::Time::now() > begin_time + Duration().fromSec(req.duration) + timeout){
+ ROS_ERROR("PointCloudSrv: Timeout waiting for laser scan to come in");
+ return false;
+ }
+ laser_lock.unlock();
+ Duration().fromSec(0.05).sleep();
+ }
+ Time end_time = laser_time_;
+ ROS_INFO("PointCloudSrv: generated point cloud from time %f to %f", begin_time.toSec(), end_time.toSec());
+
+ // get a point cloud from the point cloud assembler
+ laser_assembler::AssembleScans::Request assembler_req ;
+ laser_assembler::AssembleScans::Response assembler_res ;
+ assembler_req.begin = begin_time;
+ assembler_req.end = end_time;
+ if (!ros::service::call("laser_scan_assembler/build_cloud", assembler_req, assembler_res))
+ ROS_ERROR("PointCloudSrv: error receiving point cloud from point cloud assembler");
+ else
+ ROS_INFO("PointCloudSrv: received point cloud of size %i from point cloud assembler", assembler_res.cloud.points.size());
+
+ res.cloud = assembler_res.cloud;
+ return true;
+ }
+
+
+ void scannerSignalCallback()
+ {
+ boost::mutex::scoped_lock laser_lock(laser_mutex_);
+ // note time when tilt laser is pointing down
+ if (laser_scanner_signal_.signal == 1){
+ laser_time_ = laser_scanner_signal_.header.stamp;
+ }
+}
+
+} ;
+
+}
+
+
+using namespace pr2_laser_snapshotter;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ PointCloudSrv cloud_srv;
+ cloud_srv.spin();
+
+ return 0;
+}
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,145 @@
+/*********************************************************************
+* 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/node.h"
+
+// Services
+#include "laser_assembler/AssembleScans.h"
+
+// Messages
+#include "sensor_msgs/PointCloud.h"
+#include "pr2_msgs/LaserScannerSignal.h"
+
+/***
+ * This uses the point_cloud_assembler's build_cloud service call to grab all the scans/clouds between two tilt-laser shutters
+ * params
+ * * "~target_frame_id" (string) - This is the frame that the scanned data transformed into. The
+ * output clouds are also published in this frame.
+ * * "~num_skips" (int) - If set to N>0, then the snapshotter will skip N signals before
+ * requesting a new snapshot. This will make the snapshots be N times
+ * larger. Default 0 - no skipping.
+ */
+
+namespace pr2_laser_snapshotter
+{
+
+class PR2LaserSnapshotter
+{
+
+public:
+ ros::NodeHandle n_;
+ ros::Publisher pub_;
+ ros::Subscriber sub_;
+
+ pr2_msgs::LaserScannerSignal prev_signal_;
+
+ bool first_time_;
+
+ int num_skips_;
+ int num_skips_left_;
+
+ std::string fixed_frame_;
+
+ PR2LaserSnapshotter()
+ {
+ prev_signal_.header.stamp.fromNSec(0);
+
+ pub_ = n_.advertise<sensor_msgs::PointCloud> ("full_cloud", 1);
+ sub_ = n_.subscribe("laser_scanner_signal", 40, &PR2LaserSnapshotter::scannerSignalCallback, this);
+
+ n_.param("~num_skips", num_skips_, 0);
+ num_skips_left_=num_skips_;
+
+ first_time_ = true;
+ }
+
+ ~PR2LaserSnapshotter()
+ {
+
+ }
+
+ void scannerSignalCallback(const pr2_msgs::LaserScannerSignalConstPtr& cur_signal)
+ {
+ if (cur_signal->signal == 128 || cur_signal->signal == 129) // These codes imply that this is the first signal during a given profile type
+ first_time_ = true;
+
+
+ if (first_time_)
+ {
+ prev_signal_ = *cur_signal;
+ first_time_ = false;
+ }
+ else
+ {
+ if(num_skips_>0)
+ {
+ if(num_skips_left_>0)
+ {
+ num_skips_left_ -= 1;
+ return;
+ }
+ else
+ {
+ num_skips_left_=num_skips_;
+ }
+ }
+
+ laser_assembler::AssembleScans::Request req;
+ laser_assembler::AssembleScans::Response resp;
+
+ req.begin = prev_signal_.header.stamp;
+ req.end = cur_signal->header.stamp;
+
+ if (!ros::service::call("build_cloud", req, resp))
+ ROS_ERROR("Failed to call service on point cloud assembler or laser scan assembler.");
+
+ pub_.publish(resp.cloud);
+ ROS_DEBUG("Snapshotter::Published Cloud size=%u", resp.cloud.get_points_size());
+
+ prev_signal_ = *cur_signal;
+ }
+ }
+} ;
+
+}
+
+using namespace pr2_laser_snapshotter;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "pr2_laser_snapshotter");
+ PR2LaserSnapshotter snapshotter ;
+ ros::spin();
+ return 0;
+}
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/srv/BuildCloudAngle.srv 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,5 @@
+float64 angle_begin
+float64 angle_end
+float64 duration
+---
+sensor_msgs/PointCloud cloud
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,121 @@
+/*********************************************************************
+* 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 */
+
+/**
+ * Generate dummy scans in order to not be dependent on bag files in order to run tests
+ **/
+
+#include <boost/thread.hpp>
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <sensor_msgs/LaserScan.h>
+#include <pr2_msgs/LaserScannerSignal.h>
+
+
+void runLoop()
+{
+ ros::NodeHandle nh;
+
+ ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("dummy_scan", 100);
+ ros::Publisher signal_pub = nh.advertise<pr2_msgs::LaserScannerSignal>("dummy_signal", 100);
+ ros::Rate loop_rate(20);
+
+ // Configure the Transform broadcaster
+ tf::TransformBroadcaster broadcaster;
+ tf::Transform laser_transform(btQuaternion(0,0,0,1));
+
+ // Populate the dummy laser scan
+ sensor_msgs::LaserScan scan;
+ scan.header.frame_id = "dummy_laser_link";
+ scan.angle_min = 0.0;
+ scan.angle_max = 99.0;
+ scan.angle_increment = 1.0;
+ scan.time_increment = .001;
+ scan.scan_time = .05;
+ scan.range_min = .01;
+ scan.range_max = 100.0;
+
+ const unsigned int N = 100;
+ scan.ranges.resize(N);
+ scan.intensities.resize(N);
+
+ for (unsigned int i=0; i<N; i++)
+ {
+ scan.ranges[i] = 10.0;
+ scan.intensities[i] = 10.0;
+ }
+
+ unsigned int cloud_count = 0;
+
+ // Immeadiately send out a starting scan
+ pr2_msgs::LaserScannerSignal signal;
+ signal.header.stamp = scan.header.stamp;
+ signal.signal = 1;
+ signal_pub.publish(signal);
+
+ // Loop for each cloud
+ //while (nh.ok() & cloud_count < 1000)
+ while (nh.ok())
+ {
+ unsigned int scan_count = 0;
+ // Loop for each scan
+ while (nh.ok() & scan_count < 10)
+ {
+ scan.header.stamp = ros::Time::now();
+ scan_pub.publish(scan);
+ broadcaster.sendTransform(laser_transform, scan.header.stamp, "dummy_laser_link", "dummy_base_link");
+ scan_count++;
+ loop_rate.sleep();
+ }
+
+ signal.header.stamp = scan.header.stamp;
+ signal.signal = cloud_count % 2;
+ signal_pub.publish(signal);
+
+ ROS_INFO("Done publishing cloud [%u]", cloud_count);
+ cloud_count++;
+ }
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "scan_producer");
+ ros::NodeHandle nh;
+ boost::thread run_thread(&runLoop);
+ ros::spin();
+ run_thread.join();
+ return 0;
+}
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_assembler.cpp (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/test/test_assembler.cpp)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_assembler.cpp (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_assembler.cpp 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,140 @@
+/*********************************************************************
+* 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 */
+
+#include <string>
+#include <gtest/gtest.h>
+#include "ros/node.h"
+#include "sensor_msgs/PointCloud.h"
+#include "sensor_msgs/LaserScan.h"
+#include "boost/thread.hpp"
+
+using namespace ros;
+using namespace sensor_msgs;
+
+int g_argc;
+char** g_argv;
+
+class TestAssembler : public testing::Test
+{
+public:
+
+ NodeHandle n_;
+
+ sensor_msgs::PointCloud cloud_msg_ ;
+ boost::mutex cloud_mutex_ ;
+ sensor_msgs::PointCloud safe_cloud_ ;
+ int cloud_counter_ ;
+
+ LaserScan scan_msg_ ;
+ boost::mutex scan_mutex_ ;
+ LaserScan safe_scan_ ;
+ int scan_counter_ ;
+
+
+ void CloudCallback(const PointCloudConstPtr& cloud_msg)
+ {
+ cloud_mutex_.lock() ;
+ cloud_counter_++ ;
+ safe_cloud_ = *cloud_msg ;
+ cloud_mutex_.unlock() ;
+ ROS_INFO("Got Cloud with %u points", cloud_msg_.get_points_size()) ;
+ }
+
+ void ScanCallback(const LaserScanConstPtr& scan_msg)
+ {
+ scan_mutex_.lock() ;
+ scan_counter_++ ;
+ safe_scan_ = *scan_msg ;
+ scan_mutex_.unlock() ;
+ //ROS_INFO("Got Scan") ;
+ }
+
+protected:
+ /// constructor
+ TestAssembler()
+ {
+ cloud_counter_ = 0 ;
+ scan_counter_ = 0 ;
+ }
+
+ /// Destructor
+ ~TestAssembler()
+ {
+
+ }
+};
+
+
+TEST_F(TestAssembler, test)
+{
+ ROS_INFO("Starting test F");
+ ros::Subscriber cloud_sub = n_.subscribe("dummy_cloud", 10, &TestAssembler::CloudCallback, (TestAssembler*)this);
+ ros::Subscriber scan_sub = n_.subscribe("dummy_scan", 10, &TestAssembler::ScanCallback, (TestAssembler*)this);
+
+ bool waiting = true;
+ while(n_.ok() && waiting )
+ {
+ ros::spinOnce();
+ usleep(1e2) ;
+
+ scan_mutex_.lock() ;
+ waiting = (scan_counter_ < 55) ;
+ scan_mutex_.unlock() ;
+ }
+
+ usleep(1e6) ;
+
+ ASSERT_GT(cloud_counter_, 0) ;
+
+ unsigned int cloud_size ;
+ cloud_mutex_.lock() ;
+ cloud_size = safe_cloud_.get_points_size() ;
+ cloud_mutex_.unlock() ;
+
+ EXPECT_EQ((unsigned int) 1000, cloud_size) ;
+
+ SUCCEED();
+}
+
+int main(int argc, char** argv)
+{
+ printf("******* Starting application *********\n");
+
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "test_assembler_node");
+
+ return RUN_ALL_TESTS();
+}
Copied: pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch (from rev 23639, pkg/trunk/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch 2009-09-02 02:18:28 UTC (rev 23640)
@@ -0,0 +1,21 @@
+<launch>
+
+ <node pkg="laser_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
+ <remap from="scan_in" to="dummy_scan"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="max_scans" type="int" value="400" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <param name="fixed_frame" type="string" value="dummy_base_link" />
+ </node>
+
+ <node pkg="pr2_laser_snapshotter" type="pr2_laser_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="dummy_signal"/>
+ <remap from="build_cloud" to="laser_scan_assembler/build_cloud" />
+ <remap from="full_cloud" to="dummy_cloud" />
+ </node>
+
+ <node pkg="pr2_laser_snapshotter" type="dummy_scan_producer" output="screen"/>
+
+ <test test-name="test_laser_assembler" pkg="pr2_laser_snapshotter" type="test_assembler" />
+
+</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
