Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23424] pkg/branches/laser_pipeline_stack_fix
|
From: <vij...@us...> - 2009-08-31 22:30:32
|
Revision: 23424
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23424&view=rev
Author: vijaypradeep
Date: 2009-08-31 22:30:24 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
renaming point_cloud_assembler to laser_assembler. Updating manifests. still need to update launch files
Modified Paths:
--------------
pkg/branches/laser_pipeline_stack_fix/demos/2dnav_gazebo/manifest.xml
pkg/branches/laser_pipeline_stack_fix/demos/plug_in/manifest.xml
pkg/branches/laser_pipeline_stack_fix/demos/test_2dnav_gazebo/manifest.xml
pkg/branches/laser_pipeline_stack_fix/highlevel/doors/doors_core/manifest.xml
pkg/branches/laser_pipeline_stack_fix/highlevel/test_executive_trex_pr2/manifest.xml
pkg/branches/laser_pipeline_stack_fix/mapping/annotated_planar_patch_map/manifest.xml
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/CMakeLists.txt
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/manifest.xml
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp
pkg/branches/laser_pipeline_stack_fix/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
pkg/branches/laser_pipeline_stack_fix/sandbox/3dnav_pr2/manifest.xml
pkg/branches/laser_pipeline_stack_fix/sandbox/doors_planner_core/manifest.xml
pkg/branches/laser_pipeline_stack_fix/sandbox/labeled_object_detector/manifest.xml
pkg/branches/laser_pipeline_stack_fix/sandbox/pcd_novelty/manifest.xml
pkg/branches/laser_pipeline_stack_fix/sandbox/point_cloud_basics_demo/manifest.xml
pkg/branches/laser_pipeline_stack_fix/stacks/semantic_mapping/door_handle_detector/manifest.xml
pkg/branches/laser_pipeline_stack_fix/stacks/trex/trex_pr2_writing_test/manifest.xml
pkg/branches/laser_pipeline_stack_fix/stacks/visual_feature_detectors/outlet_detection/manifest.xml
pkg/branches/laser_pipeline_stack_fix/vision/recognition_lambertian/manifest.xml
Added Paths:
-----------
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_assembler.cpp
pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/CMakeLists.txt
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/Makefile
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/include/
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/include/laser_assembler/
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/include/laser_assembler/base_assembler_srv.h
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/mainpage.dox
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/manifest.xml
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/grab_cloud_data.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/laser_scan_assembler_srv.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/merge_clouds.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/point_cloud_assembler/
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/point_cloud_assembler/__init__.py
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/point_cloud_assembler_srv.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/srv/
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/srv/BuildCloud.srv
Removed Paths:
-------------
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/CMakeLists.txt
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/Makefile
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/mainpage.dox
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/manifest.xml
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/grab_cloud_data.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/merge_clouds.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/point_cloud_assembler/__init__.py
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/point_cloud_assembler_srv.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/srv/BuildCloud.srv
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/test/dummy_scan_producer.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/test/test_assembler.cpp
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/test/test_cloud_assembler.launch
pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/test/test_laser_assembler.launch
Modified: pkg/branches/laser_pipeline_stack_fix/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/demos/2dnav_gazebo/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/demos/2dnav_gazebo/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -17,7 +17,7 @@
<depend package="trex_pr2"/>
<depend package="robot_pose_ekf"/>
<depend package="pr2_gazebo"/>
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<depend package="semantic_point_annotator"/>
<depend package="or_robot_self_filter"/>
<depend package="robot_self_filter"/>
Modified: pkg/branches/laser_pipeline_stack_fix/demos/plug_in/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/demos/plug_in/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/demos/plug_in/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -15,7 +15,7 @@
<depend package="pr2_etherCAT" />
<depend package="joy" />
<depend package="outlet_detection" />
- <depend package="point_cloud_assembler" />
+ <depend package="laser_assembler" />
<depend package="pr2_experimental_controllers" />
<depend package="plug_onbase_detector" />
<depend package="plugs_msgs"/>
Modified: pkg/branches/laser_pipeline_stack_fix/demos/test_2dnav_gazebo/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/demos/test_2dnav_gazebo/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/demos/test_2dnav_gazebo/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -13,7 +13,7 @@
<depend package="robot_pose_ekf"/>
<depend package="pr2_gazebo"/>
<depend package="tf"/>
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<depend package="std_msgs"/>
<depend package="robot_actions"/>
<depend package="pr2_robot_actions"/>
Modified: pkg/branches/laser_pipeline_stack_fix/highlevel/doors/doors_core/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/highlevel/doors/doors_core/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/highlevel/doors/doors_core/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -31,7 +31,7 @@
<depend package="costmap_2d"/>
<depend package="base_local_planner"/>
<depend package="door_functions"/>
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<export>
<cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib "/>
Modified: pkg/branches/laser_pipeline_stack_fix/highlevel/test_executive_trex_pr2/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/highlevel/test_executive_trex_pr2/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/highlevel/test_executive_trex_pr2/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -17,7 +17,7 @@
<depend package="stage"/>
<depend package="pr2_gazebo"/>
<depend package="or_robot_self_filter"/>
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<depend package="collision_map"/>
<depend package="laser_scan"/>
<depend package="ompl_planning"/>
Modified: pkg/branches/laser_pipeline_stack_fix/mapping/annotated_planar_patch_map/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/mapping/annotated_planar_patch_map/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/mapping/annotated_planar_patch_map/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -20,7 +20,7 @@
<depend package="rostest"/>
<depend package="point_cloud_mapping" />
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<depend package="cv_mech_turk" />
Modified: pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/CMakeLists.txt
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/CMakeLists.txt 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/CMakeLists.txt 2009-08-31 22:30:24 UTC (rev 23424)
@@ -21,6 +21,16 @@
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)
Modified: pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -16,7 +16,7 @@
<depend package="geometry_msgs" />
<!-- laser_pipeline -->
- <depend package="point_cloud_assembler" />
+ <depend package="laser_assembler" />
</package>
Modified: pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/point_cloud_srv.cpp 2009-08-31 22:30:24 UTC (rev 23424)
@@ -34,7 +34,7 @@
#include "ros/node.h"
// Services
-#include "point_cloud_assembler/BuildCloud.h"
+#include "laser_assembler/BuildCloud.h"
#include "pr2_laser_snapshotter/BuildCloudAngle.h"
#include "pr2_msgs/SetPeriodicCmd.h"
@@ -52,7 +52,7 @@
* output clouds are also published in this frame.
*/
-namespace point_cloud_assembler
+namespace pr2_laser_snapshotter
{
using namespace ros;
@@ -116,8 +116,8 @@
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
- BuildCloud::Request assembler_req ;
- BuildCloud::Response assembler_res ;
+ laser_assembler::BuildCloud::Request assembler_req ;
+ laser_assembler::BuildCloud::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))
@@ -144,7 +144,7 @@
}
-using namespace point_cloud_assembler ;
+using namespace pr2_laser_snapshotter;
int main(int argc, char **argv)
{
Modified: pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/src/pr2_laser_snapshotter.cpp 2009-08-31 22:30:24 UTC (rev 23424)
@@ -35,7 +35,7 @@
#include "ros/node.h"
// Services
-#include "point_cloud_assembler/BuildCloud.h"
+#include "laser_assembler/BuildCloud.h"
// Messages
#include "sensor_msgs/PointCloud.h"
@@ -115,8 +115,8 @@
}
}
- point_cloud_assembler::BuildCloud::Request req;
- point_cloud_assembler::BuildCloud::Response resp;
+ laser_assembler::BuildCloud::Request req;
+ laser_assembler::BuildCloud::Response resp;
req.begin = prev_signal_.header.stamp;
req.end = cur_signal->header.stamp;
Copied: pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/test/dummy_scan_producer.cpp)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/dummy_scan_producer.cpp 2009-08-31 22:30:24 UTC (rev 23424)
@@ -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/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_assembler.cpp (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/test/test_assembler.cpp)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_assembler.cpp (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_assembler.cpp 2009-08-31 22:30:24 UTC (rev 23424)
@@ -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/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/test/test_laser_assembler.launch)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/pr2/pr2_laser_snapshotter/test/test_laser_assembler.launch 2009-08-31 22:30:24 UTC (rev 23424)
@@ -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>
Modified: pkg/branches/laser_pipeline_stack_fix/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -13,5 +13,5 @@
<depend package="pr2_defs" />
<depend package="pr2_default_controllers" />
<depend package="pr2_etherCAT"/>
- <depend package="point_cloud_assembler" />
+ <depend package="laser_assembler" />
</package>
Modified: pkg/branches/laser_pipeline_stack_fix/sandbox/3dnav_pr2/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/sandbox/3dnav_pr2/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/sandbox/3dnav_pr2/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -27,7 +27,7 @@
<!-- mapping -->
<depend package="laser_scan"/>
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<depend package="robot_self_filter"/>
<depend package="collision_map"/>
Modified: pkg/branches/laser_pipeline_stack_fix/sandbox/doors_planner_core/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/sandbox/doors_planner_core/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/sandbox/doors_planner_core/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -31,7 +31,7 @@
<depend package="costmap_2d"/>
<depend package="base_local_planner"/>
<depend package="door_functions"/>
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<depend package="move_arm"/>
<depend package="move_base"/>
Modified: pkg/branches/laser_pipeline_stack_fix/sandbox/labeled_object_detector/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/sandbox/labeled_object_detector/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/sandbox/labeled_object_detector/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -11,7 +11,7 @@
<depend package="robot_actions" />
<depend package="pr2_robot_actions" />
<depend package="point_cloud_mapping" />
- <depend package="point_cloud_assembler" />
+ <depend package="laser_assembler" />
<depend package="tf" />
<depend package="visualization_msgs" />
<depend package="object_names"/>
Modified: pkg/branches/laser_pipeline_stack_fix/sandbox/pcd_novelty/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/sandbox/pcd_novelty/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/sandbox/pcd_novelty/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -11,7 +11,7 @@
<depend package="robot_actions" />
<depend package="pr2_robot_actions" />
<depend package="point_cloud_mapping" />
- <depend package="point_cloud_assembler" />
+ <depend package="laser_assembler" />
<depend package="tf" />
Modified: pkg/branches/laser_pipeline_stack_fix/sandbox/point_cloud_basics_demo/manifest.xml
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/sandbox/point_cloud_basics_demo/manifest.xml 2009-08-31 22:20:10 UTC (rev 23423)
+++ pkg/branches/laser_pipeline_stack_fix/sandbox/point_cloud_basics_demo/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -11,7 +11,7 @@
<depend package="rviz"/>
<depend package="rosrecord"/>
- <depend package="point_cloud_assembler"/>
+ <depend package="laser_assembler"/>
<depend package="planar_patch_map"/>
<depend package="rosgraph"/>
Added: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/CMakeLists.txt
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/CMakeLists.txt (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/CMakeLists.txt 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rosbuild_init()
+rosbuild_gensrv()
+
+rosbuild_add_boost_directories()
+
+rosbuild_add_executable(laser_scan_assembler_srv src/laser_scan_assembler_srv.cpp)
+rosbuild_add_executable(point_cloud_assembler_srv src/point_cloud_assembler_srv.cpp)
+rosbuild_add_executable(grab_cloud_data src/grab_cloud_data.cpp)
+
+rosbuild_add_executable(merge_clouds src/merge_clouds.cpp)
+rosbuild_link_boost(merge_clouds thread)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
Copied: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/Makefile (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/Makefile)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/Makefile (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/Makefile 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Copied: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/include/laser_assembler/base_assembler_srv.h (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/include/laser_assembler/base_assembler_srv.h (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/include/laser_assembler/base_assembler_srv.h 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1,321 @@
+/*********************************************************************
+* 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 "ros/node.h"
+#include "tf/transform_listener.h"
+#include "tf/message_notifier.h"
+#include "sensor_msgs/PointCloud.h"
+
+#include <deque>
+
+// Service
+#include "laser_assembler/BuildCloud.h"
+
+#include "boost/thread.hpp"
+#include "math.h"
+
+namespace laser_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
+ *
+ * @section parameters ROS Parameters
+ *
+ * Reads the following parameters from the parameter server
+ * - \b "~tf_cache_time_secs" (double) - The cache time (in seconds) to holds past transforms
+ * - \b "~tf_tolerance_secs (double) - The time (in seconds) to wait after the transform for scan_in is available.
+ * - \b "~max_scans" (unsigned int) - The number of scans to store in the assembler's history, until they're thrown away
+ * - \b "~fixed_frame" (string) - The frame to which received data should immeadiately be transformed to
+ * - \b "~downsampling_factor" (int) - Specifies how often to sample from a scan. 1 preserves all the data. 3 keeps only 1/3 of the points.
+ *
+ * @section services ROS Service Calls
+ * - \b "~build_cloud" (BuildCloud.srv) - Accumulates scans between begin time and
+ * end time and returns the aggregated data as a point cloud
+ */
+template<class T>
+class BaseAssemblerSrv
+{
+public:
+ BaseAssemblerSrv(const std::string& node_name) ;
+ ~BaseAssemblerSrv() ;
+
+ /**
+ * \brief Tells the assembler to start listening to input data
+ * It is possible that a derived class needs to initialize and configure
+ * various things before actually beginning to process scans. Calling start
+ * create subcribes to the input stream, thus allowing the scanCallback and
+ * ConvertToCloud to be called. Start should be called only once.
+ */
+ void start() ;
+
+
+ /** \brief Returns the number of points in the current scan
+ * \param scan The scan for for which we want to know the number of points
+ * \return the number of points in scan
+ */
+ virtual unsigned int GetPointsInScan(const T& scan) = 0 ;
+
+ /** \brief Converts the current scan into a cloud in the specified fixed frame
+ *
+ * Note: Once implemented, ConvertToCloud should NOT catch TF exceptions. These exceptions are caught by
+ * BaseAssemblerSrv, and will be counted for diagnostic information
+ * \param fixed_frame_id The name of the frame in which we want cloud_out to be in
+ * \param scan_in The scan that we want to convert
+ * \param cloud_out The result of transforming scan_in into a cloud in frame fixed_frame_id
+ */
+ virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ;
+
+protected:
+ tf::TransformListener* tf_ ;
+ ros::Node node_ ;
+
+private:
+ //! \brief Callback function for every time we receive a new scan
+ //void scansCallback(const tf::MessageNotifier<T>::MessagePtr& scan_ptr, const T& testA)
+ void scansCallback(const boost::shared_ptr<T>& scan_ptr) ;
+
+ //! \brief Service Callback function called whenever we need to build a cloud
+ bool buildCloud(BuildCloud::Request& req, BuildCloud::Response& resp) ;
+
+ tf::MessageNotifier<T>* scan_notifier_ ;
+
+ //! \brief Stores history of scans
+ std::deque<sensor_msgs::PointCloud> scan_hist_ ;
+ boost::mutex scan_hist_mutex_ ;
+
+ //! \brief The number points currently in the scan history
+ unsigned int total_pts_ ;
+
+ //! \brief The max number of scans to store in the scan history
+ unsigned int max_scans_ ;
+
+ //! \brief The frame to transform data into upon receipt
+ std::string fixed_frame_ ;
+
+ //! \brief How long we should wait before processing the input data. Very useful for laser scans.
+ double tf_tolerance_secs_ ;
+
+ //! \brief Specify how much to downsample the data. A value of 1 preserves all the data. 3 would keep 1/3 of the data.
+ unsigned int downsample_factor_ ;
+
+} ;
+
+template <class T>
+BaseAssemblerSrv<T>::BaseAssemblerSrv(const std::string& node_name) : node_(node_name)
+{
+ // **** Initialize TransformListener ****
+ double tf_cache_time_secs ;
+ ros::Node::instance()->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) ;
+
+ tf_ = new tf::TransformListener(*ros::Node::instance(), true, ros::Duration(tf_cache_time_secs)) ;
+ ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
+
+ // ***** Set max_scans *****
+ const int default_max_scans = 400 ;
+ int tmp_max_scans ;
+ ros::Node::instance()->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 fixed_frame *****
+ ros::Node::instance()->param("~fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME")) ;
+ ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()) ;
+ if (fixed_frame_ == "ERROR_NO_NAME")
+ ROS_ERROR("Need to set parameter fixed_frame") ;
+
+ // ***** Set downsample_factor *****
+ int tmp_downsample_factor ;
+ ros::Node::instance()->param("~downsample_factor", tmp_downsample_factor, 1) ;
+ if (tmp_downsample_factor < 1)
+ {
+ ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor) ;
+ tmp_downsample_factor = 1 ;
+ }
+ downsample_factor_ = tmp_downsample_factor ;
+ ROS_INFO("Downsample Factor: %u", downsample_factor_) ;
+
+ // ***** Start Services *****
+ ros::Node::instance()->advertiseService(ros::Node::instance()->getName()+"/build_cloud", &BaseAssemblerSrv<T>::buildCloud, this, 0) ;
+
+ // **** Get the TF Notifier Tolerance ****
+ ros::Node::instance()->param("~tf_tolerance_secs", tf_tolerance_secs_, 0.0) ;
+ if (tf_tolerance_secs_ < 0)
+ ROS_ERROR("Parameter tf_tolerance_secs<0 (%f)", tf_tolerance_secs_) ;
+ ROS_INFO("tf Tolerance: %f seconds", tf_tolerance_secs_) ;
+
+ // ***** Start Listening to Data *****
+ // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called)
+ scan_notifier_ = NULL ;
+
+}
+
+template <class T>
+void BaseAssemblerSrv<T>::start()
+{
+ ROS_INFO("Starting to listen on the input stream") ;
+ if (scan_notifier_)
+ ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ;
+ else
+ {
+ scan_notifier_ = new tf::MessageNotifier<T>(tf_, ros::Node::instance(), boost::bind(&BaseAssemblerSrv<T>::scansCallback, this, _1), "scan_in", fixed_frame_, 10) ;
+ scan_notifier_->setTolerance(ros::Duration(tf_tolerance_secs_)) ;
+ }
+}
+
+template <class T>
+BaseAssemblerSrv<T>::~BaseAssemblerSrv()
+{
+ if (scan_notifier_)
+ delete scan_notifier_ ;
+
+ ros::Node::instance()->unadvertiseService(ros::Node::instance()->getName()+"/build_cloud") ;
+ delete tf_ ;
+}
+
+template <class T>
+void BaseAssemblerSrv<T>::scansCallback(const boost::shared_ptr<T>& scan_ptr)
+{
+ const T scan = *scan_ptr ;
+
+ sensor_msgs::PointCloud cur_cloud ;
+
+ // Convert the scan data into a universally known datatype: PointCloud
+ try
+ {
+ ConvertToCloud(fixed_frame_, scan, cur_cloud) ; // Convert scan into a point cloud
+ }
+ catch(tf::TransformException& ex)
+ {
+ ROS_WARN("Transform Exception %s", ex.what()) ;
+ return ;
+ }
+
+ // Add the current scan (now of type PointCloud) into our history of scans
+ scan_hist_mutex_.lock() ;
+ if (scan_hist_.size() == max_scans_) // Is our deque full?
+ {
+ total_pts_ -= scan_hist_.front().get_points_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(cur_cloud) ; // Add the newest scan to the back of the deque
+ total_pts_ += cur_cloud.get_points_size() ; // Add the new scan to the running total of points
+
+ //printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ;
+
+ scan_hist_mutex_.unlock() ;
+}
+
+template <class T>
+bool BaseAssemblerSrv<T>::buildCloud(BuildCloud::Request& req, BuildCloud::Response& resp)
+{
+ //printf("Starting Service Request\n") ;
+
+ scan_hist_mutex_.lock() ;
+ // Determine where in our history we actually are
+ 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 req_pts = 0 ; // Keep a total of the points in the current request
+ // Find the end of the request
+ 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
+ {
+ req_pts += (scan_hist_[i].get_points_size()+downsample_factor_-1)/downsample_factor_ ;
+ i += downsample_factor_ ;
+ }
+ unsigned int past_end_index = i ;
+
+ if (start_index == past_end_index)
+ {
+ resp.cloud.header.frame_id = fixed_frame_ ;
+ resp.cloud.header.stamp = req.end ;
+ resp.cloud.set_points_size(0) ;
+ resp.cloud.set_channels_size(0) ;
+ }
+ else
+ {
+ // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen
+ // Allocate space for the cloud
+ resp.cloud.set_points_size( req_pts ) ;
+ const unsigned int num_channels = scan_hist_[start_index].get_channels_size() ;
+ resp.cloud.set_channels_size(num_channels) ;
+ for (i = 0; i<num_channels; i++)
+ {
+ resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name ;
+ resp.cloud.channels[i].set_values_size(req_pts) ;
+ }
+ //resp.cloud.header.stamp = req.end ;
+ resp.cloud.header.frame_id = fixed_frame_ ;
+ unsigned int cloud_count = 0 ;
+ for (i=start_index; i<past_end_index; i+=downsample_factor_)
+ {
+ for(unsigned int j=0; j<scan_hist_[i].get_points_size(); j+=downsample_factor_)
+ {
+ resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x ;
+ resp.cloud.points[cloud_count].y = scan_hist_[i].points[j].y ;
+ resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z ;
+ for (unsigned int k=0; k<num_channels; k++)
+ resp.cloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ;
+
+ cloud_count++ ;
+ }
+ resp.cloud.header.stamp = scan_hist_[i].header.stamp;
+ }
+ }
+ scan_hist_mutex_.unlock() ;
+
+ ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: %u. Points in cloud: %u", start_index, past_end_index, scan_hist_.size(), resp.cloud.points.size()) ;
+ return true ;
+}
+
+}
Copied: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/mainpage.dox (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/mainpage.dox)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/mainpage.dox (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/mainpage.dox 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1,99 @@
+/**
+
+\mainpage
+
+\htmlinclude manifest.html
+
+@section summary Summary
+The point_cloud_assembler package is used to accumulate a time-series of sensor readings into a single, aggregate point cloud.
+Each sensor reading is converted into a point cloud upon receipt and then pushed onto a ring buffer. When a \b"~build_cloud"
+service request is made, the readings that lie between the start-time and end-time of the request are aggregated into a point
+cloud.
+
+
+@section usage Common Usage
+
+@subsection assemblers Assemblers
+point_cloud_assembler::BaseAssemblerSrv is an abstract class, so it cannot be used directly. Instead, a user should run one
+of the derived nodes (point_cloud_assembler::LaserScanAssemblerSrv or point_cloud_assembler::PointCloudAssemblerSrv),
+depending on what datatype they want to accumulate.
+
+@subsection snapshotter Snapshotter
+The point_cloud_assembler::PointCloudSnapshotter listens to the laser_scanner_signal messages and generates a cloud whenever
+the sensor reaches the top or bottom of it's profile. This is extremely useful when a single, consistent laser scan of the
+environment is needed.
+
+@subsection launch Example Launch File
+This launch file is performing two separate tasks
+ -# Launch a LaserScanAssemblerSrv node
+ - Listen for laser scans on topic "tilt_scan". Transform each scan (ignoring laser skew) into the torso_lift_link frame, and
+ then push the data onto a queue that is a maximum 400 elements long.
+ -# Launch the Snapshotter
+ - Listen to topic "laser_tilt_controller/laser_scanner_signal". Whenever a message is received, make a service call to
+ "laser_scan_assembler/build_cloud" to accumulate laser scans between the last two laser_scanner_signal messages. Once the
+ service call is complete, publish the data aggregate point cloud on the topic "snapshot_cloud"
+
+\verbatim
+<launch>
+
+ <node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
+ <remap from="scan_in" to="tilt_scan"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_tolerance_secs" type="double" value="0.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="torso_lift_link" />
+ <param name="downsample_factor" type="int" value="2" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="build_cloud" to="laser_scan_assembler/build_cloud" />
+ <remap from="full_cloud" to="snapshot_cloud" />
+ </node>
+
+</launch>
+
+\endverbatim
+
+@subsection merge Merging Pointclouds
+
+@b merge_clouds is a node that can take two other clouds as input,
+merge them and publish again. The frame in which the combined cloud
+is to be published (the output frame) must always be specified. If a
+maximum frequency is not specified, the node outputs clouds at the
+frequency of the input cloud that is published least often.
+
+
+A sample launch file is:
+
+\verbatim
+<launch>
+
+ <node pkg="point_cloud_assembler" type="merge_clouds" output="screen">
+
+ <!-- first input cloud -->
+ <remap from="cloud_in1" to="laser_scan_cloud"/>
+
+ <!-- second input cloud -->
+ <remap from="cloud_in2" to="stereo_scan_cloud"/>
+
+ <!-- output cloud -->
+ <remap from="cloud_out" to="merged_clouds"/>
+
+ <!-- the frame in which the output cloud should be published; this parameter must ALWAYS be specified -->
+ <param name="output_frame" type="string" value="base_link"/>
+
+ <!-- The upper bound (in Hz) of how often the merged should be published -->
+ <!-- If the value is 0.0, the cloud is published whenever updates from the input clouds are available -->
+ <param name="max_frequency" type="double" value="100.0"/>
+
+ </node>
+
+</launch>
+
+
+\endverbatim
+
+
+*/
Copied: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/manifest.xml (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/manifest.xml)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/manifest.xml (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/manifest.xml 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1,25 @@
+<package>
+ <description brief="A node for assembling point clouds out of laser scanlines.">
+ This node accumulates laser range finder scan lines into a point cloud.
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/point_cloud_assembler</url>
+ <depend package="roscpp"/>
+ <depend package="std_msgs"/>
+ <depend package="geometry_msgs"/>
+ <depend package="laser_scan"/>
+ <depend package="sensor_msgs"/>
+ <depend package="tf"/>
+ <depend package="pr2_msgs"/>
+ # For Testing
+ <depend package="rostest"/>
+ <depend package="filters"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/srv/cpp -I${prefix}/include" />
+ </export>
+
+</package>
+
Copied: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/grab_cloud_data.cpp (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/grab_cloud_data.cpp)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/grab_cloud_data.cpp (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/grab_cloud_data.cpp 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1,112 @@
+/*********************************************************************
+* 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 <cstdio>
+
+#include "ros/node.h"
+
+
+
+// Services
+#include "laser_assembler/BuildCloud.h"
+
+// Messages
+#include "sensor_msgs/PointCloud.h"
+
+
+/***
+ * This a simple test app that requests a point cloud from the point_cloud_assembler, and then publishes the resulting data
+ */
+
+namespace laser_assembler
+{
+
+class GrabCloudData : public ros::Node
+{
+
+public:
+
+ GrabCloudData() : ros::Node("grab_cloud_data")
+ {
+ advertise<sensor_msgs::PointCloud> ("full_cloud", 1) ;
+ }
+
+ ~GrabCloudData()
+ {
+ unadvertise("full_cloud") ;
+ }
+
+ bool spin()
+ {
+ ros::Duration period(4,0) ; // Repeat Every 4 seconds
+
+ ros::Time next_time = ros::Time::now() ;
+
+ while ( ok() )
+ {
+ usleep(100) ;
+ if (ros::Time::now() >= next_time)
+ {
+ next_time = (next_time + period) ;
+
+ BuildCloud::Request req ;
+ BuildCloud::Response resp ;
+
+ req.begin = (next_time - period ) ;
+ req.end = next_time ;
+
+ printf("Making Service Call...\n") ;
+ ros::service::call("build_cloud", req, resp) ;
+ printf("Done with service call\n") ;
+
+ publish("full_cloud", resp.cloud) ;
+ printf("Published Cloud size=%u\n", resp.cloud.get_points_size()) ;
+ }
+ }
+ return true ;
+ }
+} ;
+
+}
+
+using namespace laser_assembler ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ GrabCloudData grabber ;
+ grabber.spin();
+
+ return 0;
+}
Copied: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/laser_scan_assembler_srv.cpp (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/laser_scan_assembler_srv.cpp)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/laser_scan_assembler_srv.cpp (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/laser_scan_assembler_srv.cpp 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1,124 @@
+/*********************************************************************
+* 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 <laser_scan/median_filter.h>
+#include <laser_scan/intensity_filter.h>
+#include <laser_scan/scan_shadows_filter.h>
+
+#include "laser_scan/laser_scan.h"
+#include "sensor_msgs/LaserScan.h"
+#include "laser_assembler/base_assembler_srv.h"
+
+
+using namespace laser_scan;
+using namespace std ;
+
+namespace laser_assembler
+{
+
+/**
+ * \brief Maintains a history of laser scans and generates a point cloud upon request
+ * \section params ROS Parameters
+ * - (Several params are inherited from laser_assembler::BaseAssemblerSrv)
+ * - \b "~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)
+ * \section services ROS Services
+ * - "~build_cloud" - Inhertited from laser_assembler::BaseAssemblerSrv
+ */
+class LaserScanAssemblerSrv : public BaseAssemblerSrv<sensor_msgs::LaserScan>
+{
+public:
+ LaserScanAssemblerSrv() : BaseAssemblerSrv<sensor_msgs::LaserScan>("laser_scan_assembler"), filter_chain_("sensor_msgs::LaserScan")
+ {
+ // ***** Set Laser Projection Method *****
+ ros::Node::instance()->param("~ignore_laser_skew", ignore_laser_skew_, true);
+
+ // configure the filter chain from the parameter server
+ filter_chain_.configure("~filters");
+ }
+
+ ~LaserScanAssemblerSrv()
+ {
+
+ }
+
+ unsigned int GetPointsInScan(const sensor_msgs::LaserScan& scan)
+ {
+ return scan.get_ranges_size();
+ }
+
+ void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out)
+ {
+ // apply filters on laser scan
+ filter_chain_.update (scan_in, scan_filtered_);
+
+ // convert laser scan to point cloud
+ if (ignore_laser_skew_) // Do it the fast (approximate) way
+ {
+ projector_.projectLaser(scan_filtered_, cloud_out);
+ if (cloud_out.header.frame_id != fixed_frame_id)
+ tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out);
+ }
+ else // Do it the slower (more accurate) way
+ {
+ int mask = laser_scan::MASK_INTENSITY + laser_scan::MASK_DISTANCE + laser_scan::MASK_INDEX + laser_scan::MASK_TIMESTAMP;
+ projector_.transformLaserScanToPointCloud (fixed_frame_id, cloud_out, scan_filtered_, *tf_, mask);
+ }
+ return;
+ }
+
+private:
+ bool ignore_laser_skew_;
+ laser_scan::LaserProjection projector_;
+
+ filters::FilterChain<sensor_msgs::LaserScan> filter_chain_;
+ mutable sensor_msgs::LaserScan scan_filtered_;
+
+};
+
+}
+
+using namespace laser_assembler ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ LaserScanAssemblerSrv pc_assembler;
+ pc_assembler.start();
+ ros::Node::instance()->spin();
+
+ return 0;
+}
Copied: pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/merge_clouds.cpp (from rev 23423, pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/point_cloud_assembler/src/merge_clouds.cpp)
===================================================================
--- pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/merge_clouds.cpp (rev 0)
+++ pkg/branches/laser_pipeline_stack_fix/stacks/laser_pipeline/laser_assembler/src/merge_clouds.cpp 2009-08-31 22:30:24 UTC (rev 23424)
@@ -0,0 +1,196 @@
+/*********************************************************************
+* 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 condi...
[truncated message content] |
