Skip to content

Commit 89f2a51

Browse files
author
Vijay Pradeep
committed
Adding test to laser_assembler. Trac #3153
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@26843 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
1 parent ed71db1 commit 89f2a51

File tree

6 files changed

+267
-3
lines changed

6 files changed

+267
-3
lines changed

‎CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,15 @@ rosbuild_add_executable(laser_scan_assembler src/laser_scan_assembler.cpp)
1212
rosbuild_link_boost(laser_scan_assembler system)
1313
rosbuild_add_executable(point_cloud_assembler src/point_cloud_assembler.cpp)
1414

15+
rosbuild_add_executable(../test/dummy_scan_producer test/dummy_scan_producer.cpp)
1516

1617
rosbuild_add_executable(merge_clouds src/merge_clouds.cpp)
1718
rosbuild_link_boost(merge_clouds thread)
1819

20+
rosbuild_add_executable(../test/test_assembler test/test_assembler.cpp)
21+
rosbuild_add_gtest_build_flags(../test/test_assembler)
22+
23+
rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_laser_assembler.launch)
24+
1925
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
2026
add_subdirectory(examples)

‎include/laser_assembler/base_assembler_srv.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -323,7 +323,7 @@ bool BaseAssemblerSrv<T>::buildCloud(AssembleScans::Request& req, AssembleScans:
323323
}
324324
scan_hist_mutex_.unlock() ;
325325

326-
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()) ;
326+
ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: %lu. Points in cloud: %lu", start_index, past_end_index, scan_hist_.size(), resp.cloud.points.size()) ;
327327
return true ;
328328
}
329329

‎src/laser_scan_assembler.cpp‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -92,8 +92,11 @@ class LaserScanAssembler : public BaseAssembler<sensor_msgs::LaserScan>
9292
}
9393
else // Do it the slower (more accurate) way
9494
{
95-
int mask = laser_geometry::MASK_INTENSITY + laser_geometry::MASK_DISTANCE + laser_geometry::MASK_INDEX + laser_geometry::MASK_TIMESTAMP;
96-
projector_.transformLaserScanToPointCloud (fixed_frame_id, cloud_out, scan_filtered_, *tf_, mask);
95+
int mask = laser_geometry::channel_option::Intensity +
96+
laser_geometry::channel_option::Distance +
97+
laser_geometry::channel_option::Index +
98+
laser_geometry::channel_option::Timestamp;
99+
projector_.transformLaserScanToPointCloud (fixed_frame_id, scan_filtered_, cloud_out, *tf_, mask);
97100
}
98101
return;
99102
}

‎test/dummy_scan_producer.cpp‎

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2008, Willow Garage, Inc.
5+
* All rights reserved.
6+
*
7+
* Redistribution and use in source and binary forms, with or without
8+
* modification, are permitted provided that the following conditions
9+
* are met:
10+
*
11+
* * Redistributions of source code must retain the above copyright
12+
* notice, this list of conditions and the following disclaimer.
13+
* * Redistributions in binary form must reproduce the above
14+
* copyright notice, this list of conditions and the following
15+
* disclaimer in the documentation and/or other materials provided
16+
* with the distribution.
17+
* * Neither the name of the Willow Garage nor the names of its
18+
* contributors may be used to endorse or promote products derived
19+
* from this software without specific prior written permission.
20+
*
21+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+
* POSSIBILITY OF SUCH DAMAGE.
33+
*********************************************************************/
34+
35+
/* Author: Vijay Pradeep */
36+
37+
/**
38+
* Generate dummy scans in order to not be dependent on bag files in order to run tests
39+
**/
40+
41+
#include <boost/thread.hpp>
42+
#include <ros/ros.h>
43+
#include <tf/transform_broadcaster.h>
44+
#include <sensor_msgs/LaserScan.h>
45+
46+
void runLoop()
47+
{
48+
ros::NodeHandle nh;
49+
50+
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("dummy_scan", 100);
51+
ros::Rate loop_rate(5);
52+
53+
// Configure the Transform broadcaster
54+
tf::TransformBroadcaster broadcaster;
55+
tf::Transform laser_transform(btQuaternion(0,0,0,1));
56+
57+
// Populate the dummy laser scan
58+
sensor_msgs::LaserScan scan;
59+
scan.header.frame_id = "/dummy_laser_link";
60+
scan.angle_min = 0.0;
61+
scan.angle_max = 99.0;
62+
scan.angle_increment = 1.0;
63+
scan.time_increment = .001;
64+
scan.scan_time = .05;
65+
scan.range_min = .01;
66+
scan.range_max = 100.0;
67+
68+
const unsigned int N = 100;
69+
scan.ranges.resize(N);
70+
scan.intensities.resize(N);
71+
72+
for (unsigned int i=0; i<N; i++)
73+
{
74+
scan.ranges[i] = 10.0;
75+
scan.intensities[i] = 10.0;
76+
}
77+
78+
// Keep sending scans until the assembler is done
79+
while (nh.ok())
80+
{
81+
scan.header.stamp = ros::Time::now();
82+
scan_pub.publish(scan);
83+
broadcaster.sendTransform(tf::StampedTransform(laser_transform, scan.header.stamp, "dummy_laser_link", "dummy_base_link"));
84+
loop_rate.sleep();
85+
ROS_INFO("Publishing scan");
86+
}
87+
}
88+
89+
int main(int argc, char** argv)
90+
{
91+
ros::init(argc, argv, "scan_producer");
92+
ros::NodeHandle nh;
93+
boost::thread run_thread(&runLoop);
94+
ros::spin();
95+
run_thread.join();
96+
return 0;
97+
}

‎test/test_assembler.cpp‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2008, Willow Garage, Inc.
5+
* All rights reserved.
6+
*
7+
* Redistribution and use in source and binary forms, with or without
8+
* modification, are permitted provided that the following conditions
9+
* are met:
10+
*
11+
* * Redistributions of source code must retain the above copyright
12+
* notice, this list of conditions and the following disclaimer.
13+
* * Redistributions in binary form must reproduce the above
14+
* copyright notice, this list of conditions and the following
15+
* disclaimer in the documentation and/or other materials provided
16+
* with the distribution.
17+
* * Neither the name of the Willow Garage nor the names of its
18+
* contributors may be used to endorse or promote products derived
19+
* from this software without specific prior written permission.
20+
*
21+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+
* POSSIBILITY OF SUCH DAMAGE.
33+
*********************************************************************/
34+
35+
/* Author: Vijay Pradeep */
36+
37+
#include <string>
38+
#include <gtest/gtest.h>
39+
#include <ros/ros.h>
40+
#include "sensor_msgs/PointCloud.h"
41+
#include "sensor_msgs/LaserScan.h"
42+
#include "laser_assembler/AssembleScans.h"
43+
#include <boost/thread.hpp>
44+
#include <boost/thread/condition.hpp>
45+
46+
using namespace ros;
47+
using namespace sensor_msgs;
48+
using namespace std;
49+
50+
static const string SERVICE_NAME = "assemble_scans";
51+
52+
class TestAssembler : public testing::Test
53+
{
54+
public:
55+
56+
NodeHandle n_;
57+
58+
void SetUp()
59+
{
60+
ROS_INFO("Waiting for service [%s]", SERVICE_NAME.c_str());
61+
ros::service::waitForService(SERVICE_NAME);
62+
ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str());
63+
received_something_ = false;
64+
got_cloud_ = false;
65+
scan_sub_ = n_.subscribe("dummy_scan", 10, &TestAssembler::ScanCallback, (TestAssembler*)this);
66+
}
67+
68+
void ScanCallback(const LaserScanConstPtr& scan_msg)
69+
{
70+
boost::mutex::scoped_lock lock(mutex_);
71+
if (!received_something_)
72+
{
73+
// Store the time of this first scan. Will be needed when we make the service call
74+
start_time_ = scan_msg->header.stamp;
75+
received_something_ = true;
76+
}
77+
else
78+
{
79+
// Make the call to get a point cloud
80+
laser_assembler::AssembleScans assemble_scans;
81+
assemble_scans.request.begin = start_time_;
82+
assemble_scans.request.end = scan_msg->header.stamp;
83+
EXPECT_TRUE(ros::service::call(SERVICE_NAME, assemble_scans));
84+
if(assemble_scans.response.cloud.get_points_size() > 0)
85+
{
86+
ROS_INFO("Got a cloud with [%u] points. Saving the cloud", assemble_scans.response.cloud.get_points_size());
87+
cloud_ = assemble_scans.response.cloud;
88+
got_cloud_ = true;
89+
cloud_condition_.notify_all();
90+
}
91+
else
92+
ROS_INFO("Got an empty cloud. Going to try again on the next scan");
93+
}
94+
}
95+
96+
protected:
97+
ros::Subscriber scan_sub_;
98+
bool received_something_;
99+
ros::Time start_time_;
100+
bool got_cloud_;
101+
sensor_msgs::PointCloud cloud_;
102+
boost::mutex mutex_;
103+
boost::condition cloud_condition_;
104+
};
105+
106+
107+
void spinThread()
108+
{
109+
ros::spin();
110+
}
111+
112+
// Check to make sure we can get a point cloud with at least 1 point in it
113+
TEST_F(TestAssembler, non_zero_cloud_test)
114+
{
115+
// Wait until we get laser scans
116+
boost::mutex::scoped_lock lock(mutex_);
117+
118+
while(n_.ok() && !got_cloud_)
119+
{
120+
cloud_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000.0f));
121+
}
122+
123+
EXPECT_LT((unsigned int) 0, cloud_.get_points_size());
124+
125+
SUCCEED();
126+
}
127+
128+
int main(int argc, char** argv)
129+
{
130+
printf("******* Starting application *********\n");
131+
132+
testing::InitGoogleTest(&argc, argv);
133+
ros::init(argc, argv, "test_assembler_node");
134+
135+
boost::thread spin_thread(spinThread);
136+
137+
int result = RUN_ALL_TESTS();
138+
139+
ros::shutdown();
140+
141+
spin_thread.join();
142+
return result;
143+
}

‎test/test_laser_assembler.launch‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
<launch>
2+
3+
<node pkg="laser_assembler" type="dummy_scan_producer" output="screen" name="dummy_scan_producer" />
4+
5+
<node pkg="laser_assembler" type="laser_scan_assembler" output="screen" name="laser_scan_assembler">
6+
<remap from="scan" to="dummy_scan"/>
7+
<param name="tf_cache_time_secs" type="double" value="10.0" />
8+
<param name="max_scans" type="int" value="400" />
9+
<param name="ignore_laser_skew" type="bool" value="true" />
10+
<param name="fixed_frame" type="string" value="dummy_base_link" />
11+
</node>
12+
13+
<test test-name="test_laser_assembler" pkg="laser_assembler" type="test_assembler" />
14+
15+
</launch>

0 commit comments

Comments
 (0)