From 66e183be167793e5b58caed37abc6b4f519ebdef Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Jun 2019 23:32:15 +0900 Subject: [PATCH 1/3] Test matched/unmatched debug outputs --- test/CMakeLists.txt | 5 + test/src/test_debug_output.cpp | 217 +++++++++++++++++++++++++++ test/tests/debug_output_rostest.test | 24 +++ 3 files changed, 246 insertions(+) create mode 100644 test/src/test_debug_output.cpp create mode 100644 test/tests/debug_output_rostest.test diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index eb179199..87c85993 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -26,6 +26,11 @@ target_link_libraries(test_motion_prediction_model_differential_drive ${catkin_L add_executable(performance_raycast src/performance_raycast.cpp) target_link_libraries(performance_raycast ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PCL_LIBRARIES}) +add_rostest_gtest(test_debug_output tests/debug_output_rostest.test + src/test_debug_output.cpp) +target_link_libraries(test_debug_output ${catkin_LIBRARIES}) +add_dependencies(test_debug_output mcl_3dl) + add_rostest_gtest(test_expansion_resetting tests/expansion_resetting_rostest.test src/test_expansion_resetting.cpp) target_link_libraries(test_expansion_resetting ${catkin_LIBRARIES}) diff --git a/test/src/test_debug_output.cpp b/test/src/test_debug_output.cpp new file mode 100644 index 00000000..252dc58d --- /dev/null +++ b/test/src/test_debug_output.cpp @@ -0,0 +1,217 @@ +/* + * Copyright (c) 2018-2019, the mcl_3dl authors + * 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 copyright holder 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 + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace +{ +void generateSamplePointcloud2( + sensor_msgs::PointCloud2& cloud, + const float offset_x, + const float offset_y, + const float offset_z) +{ + cloud.height = 1; + cloud.is_bigendian = false; + cloud.is_dense = false; + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2Fields( + 4, + "x", 1, sensor_msgs::PointField::FLOAT32, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "intensity", 1, sensor_msgs::PointField::FLOAT32); + + class Point + { + public: + float x_, y_, z_; + Point(const float x, const float y, const float z) + : x_(x) + , y_(y) + , z_(z) + { + } + }; + std::vector points; + // Draw cube + for (float x = -1; x < 1; x += 0.05) + { + for (float y = -1; y < 1; y += 0.05) + { + points.push_back(Point(1.0 / 2 + offset_x, y + offset_y, x + offset_z)); + points.push_back(Point(-1.0 / 2 + offset_x, y + offset_y, x + offset_z)); + } + } + + modifier.resize(points.size()); + cloud.width = points.size(); + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + + for (const Point& p : points) + { + *iter_x = p.x_; + *iter_y = p.y_; + *iter_z = p.z_; + ++iter_x; + ++iter_y; + ++iter_z; + } +} + +inline sensor_msgs::PointCloud2 generateMapMsg( + const float offset_x, + const float offset_y, + const float offset_z) +{ + sensor_msgs::PointCloud2 cloud; + generateSamplePointcloud2(cloud, offset_x, offset_y, offset_z); + cloud.header.frame_id = "map"; + return cloud; +} +inline sensor_msgs::PointCloud2 generateCloudMsg() +{ + sensor_msgs::PointCloud2 cloud; + generateSamplePointcloud2(cloud, 0, 0, 0); + cloud.header.frame_id = "base_link"; + cloud.header.stamp = ros::Time::now(); + return cloud; +} +inline sensor_msgs::Imu generateImuMsg() +{ + sensor_msgs::Imu imu; + imu.header.frame_id = "base_link"; + imu.header.stamp = ros::Time::now(); + imu.orientation.w = 1; + imu.linear_acceleration.z = 9.8; + return imu; +} +inline nav_msgs::Odometry generateOdomMsg() +{ + nav_msgs::Odometry odom; + odom.header.frame_id = "odom"; + odom.header.stamp = ros::Time::now(); + odom.pose.pose.position.x = 1; + odom.pose.pose.orientation.w = 1; + return odom; +} +} // namespace + +TEST(DebugOutput, MatchedUnmatched) +{ + sensor_msgs::PointCloud2::ConstPtr matched, unmatched; + + const boost::function cb_matched = + [&matched](const sensor_msgs::PointCloud2::ConstPtr& msg) -> void + { + matched = msg; + }; + const boost::function cb_unmatched = + [&unmatched](const sensor_msgs::PointCloud2::ConstPtr& msg) -> void + { + unmatched = msg; + }; + + ros::NodeHandle nh(""); + ros::Subscriber sub_matched = nh.subscribe("mcl_3dl/matched", 1, cb_matched); + ros::Subscriber sub_unmatched = nh.subscribe("mcl_3dl/unmatched", 1, cb_unmatched); + + ros::Publisher pub_mapcloud = nh.advertise("mapcloud", 1, true); + ros::Publisher pub_cloud = nh.advertise("cloud", 1); + ros::Publisher pub_imu = nh.advertise("imu/data", 1); + ros::Publisher pub_odom = nh.advertise("odom", 1); + + const float offset_x = 1; + const float offset_y = 0; + const float offset_z = 0; + pub_mapcloud.publish(generateMapMsg(offset_x, offset_y, offset_z)); + + ros::Duration(1.0).sleep(); + ros::Rate rate(10); + for (int i = 0; i < 40; ++i) + { + rate.sleep(); + ros::spinOnce(); + if (matched && unmatched) + break; + pub_cloud.publish(generateCloudMsg()); + pub_imu.publish(generateImuMsg()); + pub_odom.publish(generateOdomMsg()); + } + ASSERT_TRUE(ros::ok()); + + ASSERT_TRUE(static_cast(matched)); + ASSERT_TRUE(static_cast(unmatched)); + + { + sensor_msgs::PointCloud2ConstIterator x(*matched, "x"); + sensor_msgs::PointCloud2ConstIterator y(*matched, "y"); + sensor_msgs::PointCloud2ConstIterator z(*matched, "z"); + for (; x != x.end(); ++x, ++y, ++z) + { + ASSERT_NEAR(*x, 0.5f, 0.1f); + ASSERT_TRUE(-1.1 < *y && *y < 1.1); + ASSERT_TRUE(-1.1 < *z && *z < 1.1); + } + } + { + sensor_msgs::PointCloud2ConstIterator x(*unmatched, "x"); + sensor_msgs::PointCloud2ConstIterator y(*unmatched, "y"); + sensor_msgs::PointCloud2ConstIterator z(*unmatched, "z"); + for (; x != x.end(); ++x, ++y, ++z) + { + ASSERT_NEAR(*x, -0.5f, 0.1f); + ASSERT_TRUE(-1.1 < *y && *y < 1.1); + ASSERT_TRUE(-1.1 < *z && *z < 1.1); + } + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_debug_output"); + + return RUN_ALL_TESTS(); +} diff --git a/test/tests/debug_output_rostest.test b/test/tests/debug_output_rostest.test new file mode 100644 index 00000000..2d8bb43c --- /dev/null +++ b/test/tests/debug_output_rostest.test @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + From 160c06d409b729e956d92efa3c9a79e219b203cb Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 18 Jun 2019 23:51:17 +0900 Subject: [PATCH 2/3] Check number of debug points --- test/src/test_debug_output.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test/src/test_debug_output.cpp b/test/src/test_debug_output.cpp index 252dc58d..76d1aa3b 100644 --- a/test/src/test_debug_output.cpp +++ b/test/src/test_debug_output.cpp @@ -184,6 +184,9 @@ TEST(DebugOutput, MatchedUnmatched) ASSERT_TRUE(static_cast(matched)); ASSERT_TRUE(static_cast(unmatched)); + ASSERT_GT(matched->width * matched->height, 0); + ASSERT_GT(unmatched->width * unmatched->height, 0); + { sensor_msgs::PointCloud2ConstIterator x(*matched, "x"); sensor_msgs::PointCloud2ConstIterator y(*matched, "y"); From f56127bdd7653c620cd07c8965c176b50cb68490 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 19 Jun 2019 00:00:13 +0900 Subject: [PATCH 3/3] Fix sign-compare warning --- test/src/test_debug_output.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/src/test_debug_output.cpp b/test/src/test_debug_output.cpp index 76d1aa3b..7b64ac37 100644 --- a/test/src/test_debug_output.cpp +++ b/test/src/test_debug_output.cpp @@ -184,8 +184,8 @@ TEST(DebugOutput, MatchedUnmatched) ASSERT_TRUE(static_cast(matched)); ASSERT_TRUE(static_cast(unmatched)); - ASSERT_GT(matched->width * matched->height, 0); - ASSERT_GT(unmatched->width * unmatched->height, 0); + ASSERT_GT(matched->width * matched->height, 0u); + ASSERT_GT(unmatched->width * unmatched->height, 0u); { sensor_msgs::PointCloud2ConstIterator x(*matched, "x");