Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8325] pkg/trunk
|
From: <vij...@us...> - 2008-12-18 07:48:30
|
Revision: 8325
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8325&view=rev
Author: vijaypradeep
Date: 2008-12-18 07:48:25 +0000 (Thu, 18 Dec 2008)
Log Message:
-----------
Lots of changes to point_cloud_assembler:
- point_cloud_assembler renamed to laser_scan_assembler_srv
- Added more parameters
- TF Exceptions for nicely handled
- No more printfs. using rosout messages instead. Better diagnostic messages being outputted
Added a PointCloudAssemblerSrv to combine clouds from filters like the ScanShadowFilter.
Removed printfs from the snapshotter.
Updated example launch scripts for building snapshots.
Fixed some scripts because of the laser_controller->laser_tilt_controller name change
Forgot to add unsubscribe call to snapshotter
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/build_snapshots.launch
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt 2008-12-18 07:25:32 UTC (rev 8324)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt 2008-12-18 07:48:25 UTC (rev 8325)
@@ -3,7 +3,8 @@
rospack(point_cloud_assembler)
gensrv()
-rospack_add_executable(point_cloud_assembler src/point_cloud_assembler.cpp)
+rospack_add_executable(laser_scan_assembler_srv src/laser_scan_assembler_srv.cpp)
+rospack_add_executable(point_cloud_assembler_srv src/point_cloud_assembler_srv.cpp)
rospack_add_executable(grab_cloud_data src/grab_cloud_data.cpp)
rospack_add_executable(point_cloud_snapshotter src/point_cloud_snapshotter.cpp)
Copied: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp (from rev 8322, pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp 2008-12-18 07:48:25 UTC (rev 8325)
@@ -0,0 +1,268 @@
+/*********************************************************************
+* 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
+
+@b LaserScanAssemblerSrv accumulates scan lines from a laser range finder mounted on a tilting stage and generates 3D point clouds.
+
+<hr>
+
+@section usage Usage
+
+To use this node, the tilting stage and laser have to be started. A convenient method for starting them is on the way, perhaps as a script.
+
+@par Example
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "scan"/<a href="../../std_msgs/html/classstd__msgs_1_1LaserScan.html">LaserScan</a> : laser scan data
+
+<hr>
+
+@section parameters ROS parameters
+
+ **/
+
+
+#include "ros/node.h"
+#include "tf/transform_listener.h"
+#include "std_msgs/LaserScan.h"
+#include "std_msgs/PointCloud.h"
+
+#include <deque>
+
+
+// Service
+#include "point_cloud_assembler/BuildCloud.h"
+
+// Laser projection
+#include "laser_scan/laser_scan.h"
+
+#include "math.h"
+
+using namespace std_msgs;
+
+using namespace std ;
+
+namespace point_cloud_assembler
+{
+
+/**
+ * \brief Maintains a history of laser scans and generates a point cloud upon request
+ * \todo Clean up the doxygen part of this header
+ * params
+ * * "~tf_cache_time_secs" (double) - The cache time (in seconds) to holds past transforms
+ * * "~tf_extrap_limit" (double) - The extrapolation limit for TF (in seconds)
+ * * "~max_scans" (unsigned int) - The number of scans to store in the assembler's history, until they're thrown away
+ * * "~ignore_laser_skew" (bool) - Specifies the method to project laser data
+ * * true -> Account for laser skew, and compute the transform for each laser point (This is currently really slow!)
+ * * false-> Don't account for laser skew, and use 1 transform per scanline. (This might have a little error when moving fast)
+ */
+class LaserScanAssemblerSrv : public ros::node
+{
+public:
+
+ tf::TransformListener* tf_;
+ laser_scan::LaserProjection projector_;
+
+ LaserScan scan_;
+
+ unsigned int max_scans_ ;
+ bool ignore_laser_skew_ ;
+
+ deque<LaserScan> scan_hist_ ; //!< Stores history of scans. We want them in time-ordered, which is (in most situations) the same as time-of-receipt-ordered
+ unsigned int total_pts_ ; //!< Stores the total number of range points in the entire stored history of scans. Useful for estimating points/scan
+
+
+ LaserScanAssemblerSrv() : ros::node("point_cloud_assembler")
+ {
+ // **** Initialize TransformListener ****
+ double tf_cache_time_secs ;
+ param("~tf_cache_time_secs", tf_cache_time_secs, 10.0) ;
+ if (tf_cache_time_secs < 0)
+ ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
+ unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL ;
+ tf_ = new tf::TransformListener(*this, true, tf_cache_time) ;
+ ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
+
+ // **** Set TF Extrapolation Limit ****
+ double tf_extrap_limit_secs ;
+ param("~tf_extrap_limit", tf_extrap_limit_secs, 0.01) ;
+ if (tf_extrap_limit_secs < 0.0)
+ ROS_ERROR("Parameter tf_extrap_limit<0 (%f)", tf_extrap_limit_secs) ;
+
+ ros::Duration extrap_limit ;
+ extrap_limit.fromSec(tf_extrap_limit_secs) ;
+ tf_->setExtrapolationLimit(extrap_limit) ;
+ ROS_INFO("TF Extrapolation Limit: %f Seconds", tf_extrap_limit_secs) ;
+
+ // ***** Set max_scans *****
+ const int default_max_scans = 400 ;
+ int tmp_max_scans;
+ param("~max_scans", tmp_max_scans, default_max_scans) ;
+ if (tmp_max_scans < 0)
+ {
+ ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ;
+ tmp_max_scans = default_max_scans ;
+ }
+ max_scans_ = tmp_max_scans;
+ ROS_INFO("Max Scans in History: %u", max_scans_) ;
+ total_pts_ = 0 ; // We're always going to start with no points in our history
+
+ // ***** Set Laser Projection Method *****
+ param("~ignore_laser_skew", ignore_laser_skew_, true) ;
+
+ // ***** Start Services *****
+ advertise_service(get_name()+"/build_cloud", &LaserScanAssemblerSrv::buildCloud, this, 0) ; // Don't spawn threads so that we can avoid dealing with mutexing [for now]
+ subscribe("scan", scan_, &LaserScanAssemblerSrv::scans_callback, 40) ; // Choose something reasonably large, so data doesn't get dropped
+ }
+
+ ~LaserScanAssemblerSrv()
+ {
+ unadvertise_service(get_name()+"/build_cloud") ;
+ unsubscribe("scan") ;
+ delete tf_ ;
+ }
+
+ void scans_callback()
+ {
+ if (scan_hist_.size() == max_scans_) // Is our deque full?
+ {
+ total_pts_ -= scan_hist_.front().get_ranges_size() ; // We're removing an elem, so this reduces our total point count
+ scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
+ }
+
+ scan_hist_.push_back(scan_) ; // Add the newest scan to the back of the deque
+ total_pts_ += scan_.get_ranges_size() ; // Add the new scan to the running total of points
+
+ //printf("LaserScanAssemblerSrv:: Got Scan: TotalPoints=%u", total_pts_) ;
+ }
+
+ bool buildCloud(BuildCloud::request& req, BuildCloud::response& resp)
+ {
+ // Allocate space for the cloud
+ resp.cloud.set_pts_size( total_pts_ ) ; // There's no way to have a point cloud bigger than our entire history of scans.
+ resp.cloud.set_chan_size(1) ;
+ resp.cloud.chan[0].name = "intensities" ;
+ resp.cloud.chan[0].set_vals_size( total_pts_ ) ;
+
+ unsigned int cloud_count = 0 ; // Store the number of points in the current cloud
+
+ PointCloud projector_cloud ; // Stores the current scan after being projected into the laser frame
+ PointCloud target_frame_cloud ; // Stores the current scan in the target frame
+
+ unsigned int i = 0 ;
+
+ // Find the beginning of the request. Probably should be a search
+ while ( i < scan_hist_.size() && // Don't go past end of deque
+ scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time
+ {
+ i++ ;
+ }
+
+ unsigned int start_index = i ;
+
+ unsigned int tf_ex_count = 0 ; // Keep a count of how many TF exceptions we got (for diagnostics)
+ unsigned int scan_lines_count = 0 ; // Keep a count of how many scan lines we agregated (for diagnostics)
+
+ // Populate the cloud
+ while ( i < scan_hist_.size() && // Don't go past end of deque
+ scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
+ {
+ const LaserScan& cur_scan = scan_hist_[i] ;
+
+ try
+ {
+ if (ignore_laser_skew_) // Do it the fast (approximate) way
+ {
+ projector_.projectLaser(cur_scan, projector_cloud) ;
+ tf_->transformPointCloud(req.target_frame_id, projector_cloud, target_frame_cloud) ;
+ }
+ else // Do it the slower (more accurate) way
+ {
+ projector_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan, *tf_) ;
+ }
+
+ for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
+ {
+ resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x ;
+ resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y ;
+ resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z ;
+ resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j] ;
+ cloud_count++ ;
+ }
+ resp.cloud.header = target_frame_cloud.header ; // Find a better place to do this/way to do this
+ }
+ catch(tf::TransformException& ex)
+ {
+ //ROS_WARN("Transform Exception %s", ex.what()) ;
+ tf_ex_count++ ;
+ }
+
+ scan_lines_count++ ;
+ i++ ; // Check the next scan in the scan history
+ }
+ int end_index = i ;
+
+ resp.cloud.set_pts_size( cloud_count ) ; // Resize the output accordingly
+ resp.cloud.chan[0].set_vals_size( cloud_count ) ;
+
+ ROS_INFO("Point Cloud Results: Agregated from index %u->%u. Total ScanLines: %u. BufferSize: %u", start_index, end_index, scan_lines_count, scan_hist_.size()) ;
+ if (tf_ex_count > 0)
+ ROS_WARN("%u TF Exceptions while generating Point Cloud", tf_ex_count) ;
+
+ return true ;
+ }
+};
+
+}
+
+using namespace point_cloud_assembler ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ LaserScanAssemblerSrv pc_assembler;
+ pc_assembler.spin();
+ ros::fini();
+ return 0;
+}
Deleted: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-12-18 07:25:32 UTC (rev 8324)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-12-18 07:48:25 UTC (rev 8325)
@@ -1,227 +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
-
-@b point_cloud_assembler accumulates scan lines from a laser range finder mounted on a tilting stage and generates 3D point clouds.
-
-<hr>
-
-@section usage Usage
-
-To use this node, the tilting stage and laser have to be started. A convenient method for starting them is on the way, perhaps as a script.
-
-@par Example
-
-<hr>
-
-@section topic ROS topics
-
-Subscribes to (name/type):
-- @b "scan"/<a href="../../std_msgs/html/classstd__msgs_1_1LaserScan.html">LaserScan</a> : laser scan data
-
-<hr>
-
-@section parameters ROS parameters
-
- **/
-
-
-#include "ros/node.h"
-#include "tf/transform_listener.h"
-#include "std_msgs/LaserScan.h"
-#include "std_msgs/PointCloud.h"
-
-#include <deque>
-
-
-// Service
-#include "point_cloud_assembler/BuildCloud.h"
-
-// Laser projection
-#include "laser_scan/laser_scan.h"
-
-#include "math.h"
-
-using namespace std_msgs;
-
-using namespace std ;
-
-namespace point_cloud_assembler
-{
-
-class PointCloudAssembler : public ros::node
-{
-public:
-
- tf::TransformListener tf_;
- laser_scan::LaserProjection projector_;
-
- LaserScan scan_;
-
- unsigned int max_scans_ ;
- bool ignore_laser_skew_ ;
-
- deque<LaserScan> scan_hist_ ; //!< Stores history of scans. We want them in time-ordered, which is (in most situations) the same as time-of-receipt-ordered
- unsigned int total_pts_ ; //!< Stores the total number of range points in the entire stored history of scans. Useful for estimating points/scan
-
-
- PointCloudAssembler() : ros::node("point_cloud_assembler"), tf_(*this)
- {
- tf_.setExtrapolationLimit(ros::Duration().fromSec(1.0)) ;
-
- advertise_service("build_cloud", &PointCloudAssembler::buildCloud, this, 0) ; // Don't spawn threads so that we can avoid dealing with mutexing [for now]
- subscribe("scan", scan_, &PointCloudAssembler::scans_callback, 40) ;
-
- int tmp_max_scans;
- param("point_cloud_assembler/max_scans", tmp_max_scans, 400) ;
- if (tmp_max_scans < 0)
- tmp_max_scans = 400;
-
- max_scans_ = tmp_max_scans;
-
- param("~ignore_laser_skew", ignore_laser_skew_, true) ;
-
- total_pts_ = 0 ; // We're always going to start with no points in our history
- }
-
- virtual ~PointCloudAssembler()
- {
- unadvertise_service("build_cloud") ;
- unsubscribe("scan") ;
- }
-
- void scans_callback()
- {
- if (scan_hist_.size() == max_scans_) // Is our deque full?
- {
- total_pts_ -= scan_hist_.front().get_ranges_size() ; // We're removing an elem, so this reduces our total point count
- scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
- }
-
- scan_hist_.push_back(scan_) ; // Add the newest scan to the back of the deque
- total_pts_ += scan_.get_ranges_size() ; // Add the new scan to the running total of points
-
- //printf("PointCloudAssembler:: Got Scan: TotalPoints=%u", total_pts_) ;
- }
-
- bool buildCloud(BuildCloud::request& req, BuildCloud::response& resp)
- {
- // Allocate space for the cloud
- resp.cloud.set_pts_size( total_pts_ ) ; // There's no way to have a point cloud bigger than our entire history of scans.
- resp.cloud.set_chan_size(1) ;
- resp.cloud.chan[0].name = "intensities" ;
- resp.cloud.chan[0].set_vals_size( total_pts_ ) ;
-
- unsigned int cloud_count = 0 ; // Store the number of points in the current cloud
-
- PointCloud projector_cloud ; // Stores the current scan after being projected into the laser frame
- PointCloud target_frame_cloud ; // Stores the current scan in the target frame
-
- unsigned int i = 0 ;
-
- // Find the beginning of the request. Probably should be a search
- while ( i < scan_hist_.size() && // Don't go past end of deque
- scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time
- {
- i++ ;
- }
-
- printf(" Start i=%u\n", i) ;
-
- // Populate the cloud
- while ( i < scan_hist_.size() && // Don't go past end of deque
- scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
- {
- const LaserScan& cur_scan = scan_hist_[i] ;
-
- try
- {
- if (ignore_laser_skew_) // Do it the fast (approximate) way
- {
- projector_.projectLaser(cur_scan, projector_cloud) ;
- tf_.transformPointCloud(req.target_frame_id, projector_cloud, target_frame_cloud) ;
- }
- else // Do it the slower (more accurate) way
- {
- projector_.transformLaserScanToPointCloud(req.target_frame_id, target_frame_cloud, cur_scan, tf_) ;
- }
-
- for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
- {
- resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x ;
- resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y ;
- resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z ;
- resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j] ;
- cloud_count++ ;
- }
- }
- catch(tf::TransformException& ex)
- {
- ROS_WARN("Transform Exception %s", ex.what()) ;
-
- return true ;
- }
-
- resp.cloud.header = target_frame_cloud.header ; // Find a better place to do this/way to do this
-
- i++ ; // Check the next scan in the scan history
- }
-
- printf(" End i=%u\n", i) ;
-
- resp.cloud.set_pts_size( cloud_count ) ; // Resize the output accordingly
- resp.cloud.chan[0].set_vals_size( cloud_count ) ;
-
- return true ;
- }
-};
-
-}
-
-using namespace point_cloud_assembler ;
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv);
- PointCloudAssembler pc_assembler;
- pc_assembler.spin();
- ros::fini();
- return 0;
-}
Copied: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp (from rev 8322, pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp)
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp 2008-12-18 07:48:25 UTC (rev 8325)
@@ -0,0 +1,218 @@
+/*********************************************************************
+* 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"
+#include "tf/transform_listener.h"
+#include "std_msgs/PointCloud.h"
+
+#include <deque>
+
+// Service
+#include "point_cloud_assembler/BuildCloud.h"
+
+// Laser projection
+#include "laser_scan/laser_scan.h"
+
+#include "math.h"
+
+using namespace std_msgs;
+
+using namespace std ;
+
+namespace point_cloud_assembler
+{
+
+/**
+ * \brief Maintains a history of point clouds and generates an aggregate point cloud upon request
+ * \todo Clean up the doxygen part of this header
+ * params
+ * * "~tf_cache_time_secs" (double) - The cache time (in seconds) to holds past transforms
+ * * "~tf_extrap_limit" (double) - The extrapolation limit for TF (in seconds)
+ * * "~max_scans" (unsigned int) - The number of scans to store in the assembler's history, until they're thrown away
+ */
+class PointCloudAssemblerSrv : public ros::node
+{
+public:
+
+ tf::TransformListener* tf_ ;
+
+ PointCloud scan_ ;
+
+ unsigned int max_scans_ ;
+ bool ignore_laser_skew_ ;
+
+ deque<PointCloud> scan_hist_ ; //!< Stores history of scans. We want them in time-ordered, which is (in most situations) the same as time-of-receipt-ordered
+ unsigned int total_pts_ ; //!< Stores the total number of range points in the entire stored history of scans. Useful for estimating points/scan
+
+ PointCloudAssemblerSrv() : ros::node("point_cloud_assembler_srv")
+ {
+ // **** Initialize TransformListener ****
+ double tf_cache_time_secs ;
+ param("~tf_cache_time_secs", tf_cache_time_secs, 10.0) ;
+ if (tf_cache_time_secs < 0)
+ ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
+ unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL ;
+ tf_ = new tf::TransformListener(*this, true, tf_cache_time) ;
+ ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
+
+ // **** Set TF Extrapolation Limit ****
+ double tf_extrap_limit_secs ;
+ param("~tf_extrap_limit", tf_extrap_limit_secs, 0.01) ;
+ if (tf_extrap_limit_secs < 0.0)
+ ROS_ERROR("Parameter tf_extrap_limit<0 (%f)", tf_extrap_limit_secs) ;
+
+ ros::Duration extrap_limit ;
+ extrap_limit.fromSec(tf_extrap_limit_secs) ;
+ tf_->setExtrapolationLimit(extrap_limit) ;
+ ROS_INFO("TF Extrapolation Limit: %f Seconds", tf_extrap_limit_secs) ;
+
+ // ***** Set max_scans *****
+ const int default_max_scans = 400 ;
+ int tmp_max_scans ;
+ param("~max_scans", tmp_max_scans, default_max_scans) ;
+ if (tmp_max_scans < 0)
+ {
+ ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ;
+ tmp_max_scans = default_max_scans ;
+ }
+ max_scans_ = tmp_max_scans ;
+ ROS_INFO("Max Scans in History: %u", max_scans_) ;
+ total_pts_ = 0 ; // We're always going to start with no points in our history
+
+ // ***** Start Services *****
+ advertise_service(get_name()+"/build_cloud", &PointCloudAssemblerSrv::buildCloud, this, 0) ; // Don't spawn threads so that we can avoid dealing with mutexing [for now]
+ subscribe("cloud_in", scan_, &PointCloudAssemblerSrv::scans_callback, 40) ; // Choose something reasonably large, so data doesn't get dropped
+ }
+
+ ~PointCloudAssemblerSrv()
+ {
+ unadvertise_service(get_name()+"/build_cloud") ;
+ unsubscribe("cloud_in") ;
+ delete tf_ ;
+ }
+
+ void scans_callback()
+ {
+ if (scan_hist_.size() == max_scans_) // Is our deque full?
+ {
+ total_pts_ -= scan_hist_.front().get_pts_size() ; // We're removing an elem, so this reduces our total point count
+ scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
+ }
+
+ scan_hist_.push_back(scan_) ; // Add the newest scan to the back of the deque
+ total_pts_ += scan_.get_pts_size() ; // Add the new scan to the running total of points
+
+ //printf("LaserScanAssemblerSrv:: Got Scan: TotalPoints=%u", total_pts_) ;
+ }
+
+ bool buildCloud(BuildCloud::request& req, BuildCloud::response& resp)
+ {
+ // Allocate space for the cloud
+ resp.cloud.set_pts_size( total_pts_ ) ; // There's no way to have a point cloud bigger than our entire history of scans.
+ resp.cloud.set_chan_size(1) ;
+ resp.cloud.chan[0].name = "intensities" ; //! \todo More smartly deal with multiple channels
+ resp.cloud.chan[0].set_vals_size( total_pts_ ) ;
+
+ unsigned int cloud_count = 0 ; // Store the number of points in the current cloud
+
+ PointCloud target_frame_cloud ; // Stores the current scan in the target frame
+
+ unsigned int i = 0 ;
+
+ // Find the beginning of the request. Probably should be a search
+ while ( i < scan_hist_.size() && // Don't go past end of deque
+ scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time
+ {
+ i++ ;
+ }
+
+ unsigned int start_index = i ;
+
+ unsigned int tf_ex_count = 0 ; // Keep a count of how many TF exceptions we got (for diagnostics)
+ unsigned int scan_lines_count = 0 ; // Keep a count of how many scan lines we aggregated (for diagnostics)
+
+ // Populate the cloud
+ while ( i < scan_hist_.size() && // Don't go past end of deque
+ scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
+ {
+ const PointCloud& cur_scan = scan_hist_[i] ;
+
+ try
+ {
+ tf_->transformPointCloud(req.target_frame_id, cur_scan, target_frame_cloud) ;
+
+ for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
+ {
+ resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x ;
+ resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y ;
+ resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z ;
+ resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j] ;
+ cloud_count++ ;
+ }
+ resp.cloud.header = target_frame_cloud.header ; //! \todo Find a better place to do this/way to do this
+ }
+ catch(tf::TransformException& ex)
+ {
+ //ROS_WARN("Transform Exception %s", ex.what()) ;
+ tf_ex_count++ ;
+ }
+
+ scan_lines_count++ ;
+ i++ ; // Check the next scan in the scan history
+ }
+ int end_index = i ;
+
+ resp.cloud.set_pts_size( cloud_count ) ; // Resize the output accordingly
+ resp.cloud.chan[0].set_vals_size( cloud_count ) ;
+
+ ROS_INFO("Point Cloud Results: Aggregated from index %u->%u. Total ScanLines: %u. BufferSize: %u", start_index, end_index, scan_lines_count, scan_hist_.size()) ;
+ if (tf_ex_count > 0)
+ ROS_WARN("%u TF Exceptions while generating Point Cloud", tf_ex_count) ;
+
+ return true ;
+ }
+};
+
+}
+
+using namespace point_cloud_assembler ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ PointCloudAssemblerSrv pc_assembler;
+ pc_assembler.spin();
+ ros::fini();
+ return 0;
+}
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-12-18 07:25:32 UTC (rev 8324)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-12-18 07:48:25 UTC (rev 8325)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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
@@ -17,7 +17,7 @@
* * 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
@@ -44,7 +44,10 @@
using namespace std_msgs ;
/***
- * This uses the point_cloud_assembler's build_cloud service call to grab all the laser scans between two tilt-laser shutters
+ * This uses the point_cloud_assembler's build_cloud service call to grab all the scans/clouds between two tilt-laser shutters
+ * params
+ * * "~fixed_frame" (string) - This is the frame that the scanned data is assumed to be stationary in. The
+ * output clouds are also published in this frame.
*/
namespace point_cloud_assembler
@@ -52,35 +55,34 @@
class PointCloudSnapshotter : public ros::node
{
-
+
public:
pr2_mechanism_controllers::LaserScannerSignal prev_signal_;
pr2_mechanism_controllers::LaserScannerSignal cur_signal_;
-
+
bool first_time_ ;
-
+
std::string fixed_frame_ ;
-
+
PointCloudSnapshotter() : ros::node("point_cloud_snapshotter")
{
prev_signal_.header.stamp.fromNSec(0) ;
-
+
advertise<PointCloud> ("full_cloud", 1) ;
subscribe("laser_scanner_signal", cur_signal_, &PointCloudSnapshotter::scannerSignalCallback, 40) ;
- std::string default_fixed_frame = "torso" ;
+ param("~fixed_frame", fixed_frame_, std::string("NO_FRAME_DEFINED")) ;
- param("~fixed_frame", fixed_frame_, std::string("torso")) ;
-
first_time_ = true ;
}
-
+
~PointCloudSnapshotter()
{
+ unsubscribe("laser_scanner_signal") ;
unadvertise("full_cloud") ;
}
-
+
void scannerSignalCallback()
{
if (first_time_)
@@ -92,18 +94,18 @@
{
BuildCloud::request req ;
BuildCloud::response resp ;
-
+
req.begin = prev_signal_.header.stamp ;
req.end = cur_signal_.header.stamp ;
req.target_frame_id = fixed_frame_ ;
-
- printf("PointCloudSnapshotter::Making Service Call...\n") ;
+
+ //printf("PointCloudSnapshotter::Making Service Call...\n") ;
ros::service::call("build_cloud", req, resp) ;
- printf("PointCloudSnapshotter::Done with service call\n") ;
-
+ //printf("PointCloudSnapshotter::Done with service call\n") ;
+
publish("full_cloud", resp.cloud) ;
- printf("PointCloudSnapshotter::Published Cloud size=%u\n", resp.cloud.get_pts_size()) ;
-
+ ROS_INFO("Snapshotter::Published Cloud size=%u", resp.cloud.get_pts_size()) ;
+
prev_signal_ = cur_signal_ ;
}
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml 2008-12-18 07:25:32 UTC (rev 8324)
+++ pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/control_laser_tilt_roslaunch.xml 2008-12-18 07:48:25 UTC (rev 8325)
@@ -1,4 +1,4 @@
<launch>
<node pkg="mechanism_control" type="spawner.py" args="laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_controller sine 15 .75 .25" />
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 15 .75 .25" />
</launch>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/build_snapshots.launch
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/build_snapshots.launch 2008-12-18 07:25:32 UTC (rev 8324)
+++ pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/build_snapshots.launch 2008-12-18 07:48:25 UTC (rev 8325)
@@ -1,12 +1,30 @@
<launch>
- <node pkg="point_cloud_assembler" type="point_cloud_assembler" output="screen" name="assembler">
+
+ <!-- Run this node if you want to aggregate laser_scans into a point cloud -->
+ <node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
<remap from="scan" to="tilt_scan"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_extrap_limit" type="double" value="0.01" />
+ <param name="max_scans" type="int" value="400" />
<param name="ignore_laser_skew" type="bool" value="true" />
</node>
+ <!-- Run this node if you want to aggregate many point_clouds into one larger point cloud -->
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
+ <remap from="cloud_in" to="tilt_laser_cloud_filtered" />
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_extrap_limit" type="double" value="0.01" />
+ <param name="max_scans" type="int" value="400" />
+ </node>
+
+ <!-- Run this node if you want clouds automatically published during laser shutter messages -->
+ call 'point_cloud_assembler/build_cloud' to aggregate point clouds
+ call 'point_cloud_assembler/build_cloud' to aggregate laser scans -->
<node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_controller/laser_scanner_signal"/>
- <param name="fixed_frame" type="string" value="torso_lift_link" />
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
+ <remap from="full_cloud" to="snapshot_cloud" />
+ <param name="fixed_frame" type="string" value="base_link" />
</node>
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
