From 80007ed4a0eecf44505ef523df43a0c4d55dc6b0 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 26 Apr 2022 03:50:39 +0900 Subject: [PATCH] fix(obstacle_collision_checker): fix bug with filterPointCloudByTrajectory (#729) * Add unit test showcasing the issue Signed-off-by: Maxime CLEMENT * Fix filterPointCloudByTrajectory Signed-off-by: Maxime CLEMENT * Add NOLINT for direct include of .cpp file Signed-off-by: Maxime CLEMENT --- .../obstacle_collision_checker/CMakeLists.txt | 4 ++ .../obstacle_collision_checker/package.xml | 1 + .../obstacle_collision_checker.cpp | 5 +- .../test/test_obstacle_collision_checker.cpp | 46 +++++++++++++++++++ 4 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp diff --git a/control/obstacle_collision_checker/CMakeLists.txt b/control/obstacle_collision_checker/CMakeLists.txt index d9747083772b6..e13747c2711e8 100644 --- a/control/obstacle_collision_checker/CMakeLists.txt +++ b/control/obstacle_collision_checker/CMakeLists.txt @@ -44,6 +44,10 @@ rclcpp_components_register_node(obstacle_collision_checker if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_obstacle_collision_checker + test/test_obstacle_collision_checker.cpp + ) + target_link_libraries(test_obstacle_collision_checker obstacle_collision_checker) endif() ament_auto_package( diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index eea21a305a81a..b7ad2264a5104 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -26,6 +26,7 @@ tier4_autoware_utils vehicle_info_util + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 79622d527b36c..8f2104e22d153 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -59,12 +59,13 @@ pcl::PointCloud filterPointCloudByTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) { pcl::PointCloud filtered_pointcloud; - for (const auto & trajectory_point : trajectory.points) { - for (const auto & point : pointcloud.points) { + for (const auto & point : pointcloud.points) { + for (const auto & trajectory_point : trajectory.points) { const double dx = trajectory_point.pose.position.x - point.x; const double dy = trajectory_point.pose.position.y - point.y; if (std::hypot(dx, dy) < radius) { filtered_pointcloud.points.push_back(point); + break; } } } diff --git a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp new file mode 100644 index 0000000000000..e851cca85c11f --- /dev/null +++ b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -0,0 +1,46 @@ +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/obstacle_collision_checker_node/obstacle_collision_checker.cpp" // NOLINT +#include "gtest/gtest.h" + +TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) +{ + pcl::PointCloud pcl; + autoware_auto_planning_msgs::msg::Trajectory trajectory; + pcl::PointXYZ pcl_point; + autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + pcl_point.y = 0.0; + traj_point.pose.position.y = 0.99; + for (double x = 0.0; x < 10.0; x += 1.0) { + pcl_point.x = x; + traj_point.pose.position.x = x; + trajectory.points.push_back(traj_point); + pcl.push_back(pcl_point); + } + // radius < 1: all points are filtered + for (auto radius = 0.0; radius <= 0.99; radius += 0.1) { + const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius); + EXPECT_EQ(filtered_pcl.size(), 0ul); + } + // radius >= 1.0: all points are kept + for (auto radius = 1.0; radius < 10.0; radius += 0.1) { + const auto filtered_pcl = filterPointCloudByTrajectory(pcl, trajectory, radius); + ASSERT_EQ(pcl.size(), filtered_pcl.size()); + for (size_t i = 0; i < pcl.size(); ++i) { + EXPECT_EQ(pcl[i].x, filtered_pcl[i].x); + EXPECT_EQ(pcl[i].y, filtered_pcl[i].y); + } + } +}