Skip to content

Commit 66c7011

Browse files
Motsu-sanMotsu-san
committed
feat: add unit tests for LidarMarkerLocalizer and implement testing framework
Signed-off-by: Motsu-san <83898149+Motsu-san@users.noreply.github.com>
1 parent 98ae8f5 commit 66c7011

3 files changed

Lines changed: 185 additions & 5 deletions

File tree

‎localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,14 @@ rclcpp_components_register_node(${PROJECT_NAME}
2727
if(BUILD_TESTING)
2828
find_package(ament_lint_auto REQUIRED)
2929
ament_lint_auto_find_test_dependencies()
30+
find_package(ament_cmake_gtest REQUIRED)
31+
ament_add_gtest(test_lidar_marker_localizer test/test_lidar_marker_localizer.cpp)
32+
target_include_directories(test_lidar_marker_localizer PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src)
33+
target_link_libraries(test_lidar_marker_localizer
34+
${PROJECT_NAME}
35+
${PCL_LIBRARIES}
36+
)
37+
ament_target_dependencies(test_lidar_marker_localizer rclcpp rclcpp_components)
3038
endif()
3139

3240
ament_auto_package(INSTALL_TO_SHARE

‎localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,11 @@ class LidarMarkerLocalizer : public rclcpp::Node
6060
using SetBool = std_srvs::srv::SetBool;
6161
using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus;
6262

63+
public:
64+
explicit LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options);
65+
66+
protected:
67+
// For test subclass access
6368
struct Param
6469
{
6570
bool enable_read_all_target_ids;
@@ -100,8 +105,11 @@ class LidarMarkerLocalizer : public rclcpp::Node
100105
int64_t queue_size_for_debug_pub_msg;
101106
};
102107

103-
public:
104-
explicit LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options);
108+
Param & mutable_param() { return param_; }
109+
const Param & param() const { return param_; }
110+
template <typename PointType>
111+
std::vector<landmark_manager::Landmark> detect_landmarks(
112+
const PointCloud2::ConstSharedPtr & points_msg_ptr);
105113

106114
private:
107115
void self_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr);
@@ -112,9 +120,6 @@ class LidarMarkerLocalizer : public rclcpp::Node
112120

113121
void initialize_diagnostics();
114122
void main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr);
115-
template <typename PointType>
116-
std::vector<landmark_manager::Landmark> detect_landmarks(
117-
const PointCloud2::ConstSharedPtr & points_msg_ptr);
118123
sensor_msgs::msg::PointCloud2::SharedPtr extract_marker_pointcloud(
119124
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr,
120125
const geometry_msgs::msg::Pose marker_pose) const;

‎localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/test/test_lidar_marker_localizer.cpp‎

Lines changed: 167 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
1+
// Copyright 2023 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "lidar_marker_localizer.hpp"
16+
17+
#include <autoware/point_types/types.hpp>
18+
19+
#include <gtest/gtest.h>
20+
21+
namespace autoware::lidar_marker_localizer
22+
{
23+
24+
class TestableLidarMarkerLocalizer : public LidarMarkerLocalizer
25+
{
26+
public:
27+
using LidarMarkerLocalizer::LidarMarkerLocalizer;
28+
29+
template <typename PointT>
30+
auto callDetectLandmarks(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr)
31+
{
32+
return this->detect_landmarks<PointT>(points_msg_ptr);
33+
}
34+
35+
Param & public_mutable_param() { return mutable_param(); }
36+
const Param & public_param() const { return param(); }
37+
};
38+
39+
class LidarMarkerLocalizerTest : public ::testing::Test
40+
{
41+
protected:
42+
rclcpp::NodeOptions node_options;
43+
std::shared_ptr<TestableLidarMarkerLocalizer> localizer;
44+
45+
void SetUp() override
46+
{
47+
node_options.parameter_overrides(
48+
{{"enable_read_all_target_ids", false},
49+
{"target_ids", std::vector<std::string>{}},
50+
{"queue_size_for_output_pose", 1},
51+
{"marker_name", std::string("")},
52+
{"road_surface_mode", false},
53+
{"resolution", 1.0},
54+
{"intensity_pattern", std::vector<int64_t>{}},
55+
{"match_intensity_difference_threshold", 1},
56+
{"positive_match_num_threshold", 1},
57+
{"negative_match_num_threshold", 1},
58+
{"vote_threshold_for_detect_marker", 1},
59+
{"marker_to_vehicle_offset_y", 0.0},
60+
{"marker_height_from_ground", 0.0},
61+
{"reference_ring_number", 0},
62+
{"marker_width", 0.8},
63+
{"self_pose_timeout_sec", 1.0},
64+
{"self_pose_distance_tolerance_m", 1.0},
65+
{"limit_distance_from_self_pose_to_nearest_marker", 1.0},
66+
{"limit_distance_from_self_pose_to_nearest_marker_y", 1.0},
67+
{"limit_distance_from_self_pose_to_marker", 1.0},
68+
{"base_covariance", std::vector<double>(36, 0.0)},
69+
{"lower_ring_id_init", 0},
70+
{"upper_ring_id_init", 0},
71+
{"enable_save_log", false},
72+
{"save_file_directory_path", std::string("")},
73+
{"save_file_name", std::string("")},
74+
{"save_frame_id", std::string("")},
75+
{"radius_for_extracting_marker_pointcloud", 1.0},
76+
{"queue_size_for_debug_pub_msg", 1}});
77+
localizer = std::make_shared<TestableLidarMarkerLocalizer>(node_options);
78+
}
79+
80+
}; // class LidarMarkerLocalizerTest : public ::testing::Test
81+
82+
TEST_F(LidarMarkerLocalizerTest, Initialization)
83+
{
84+
ASSERT_NE(localizer, nullptr);
85+
}
86+
87+
TEST_F(LidarMarkerLocalizerTest, DetectLandmarksRoadSurfaceModeSwitch)
88+
{
89+
// ダミーPointCloud2生成
90+
sensor_msgs::msg::PointCloud2 cloud;
91+
cloud.header.frame_id = "base_link";
92+
cloud.header.stamp.sec = 0;
93+
cloud.header.stamp.nanosec = 0;
94+
// 必要なフィールドをセット
95+
cloud.height = 1;
96+
cloud.width = 2;
97+
cloud.is_dense = true;
98+
cloud.is_bigendian = false;
99+
cloud.point_step =
100+
sizeof(float) * 4 + sizeof(uint16_t) + sizeof(float); // x, y, z, intensity, channel, dummy
101+
cloud.row_step = cloud.point_step * cloud.width;
102+
cloud.fields.resize(5);
103+
cloud.fields[0].name = "x";
104+
cloud.fields[0].offset = 0;
105+
cloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
106+
cloud.fields[0].count = 1;
107+
cloud.fields[1].name = "y";
108+
cloud.fields[1].offset = 4;
109+
cloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
110+
cloud.fields[1].count = 1;
111+
cloud.fields[2].name = "z";
112+
cloud.fields[2].offset = 8;
113+
cloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
114+
cloud.fields[2].count = 1;
115+
cloud.fields[3].name = "intensity";
116+
cloud.fields[3].offset = 12;
117+
cloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32;
118+
cloud.fields[3].count = 1;
119+
cloud.fields[4].name = "channel";
120+
cloud.fields[4].offset = 16;
121+
cloud.fields[4].datatype = sensor_msgs::msg::PointField::UINT16;
122+
cloud.fields[4].count = 1;
123+
cloud.data.resize(cloud.row_step * cloud.height);
124+
125+
// パラメータセット
126+
auto & param = localizer->public_mutable_param();
127+
param.lower_ring_id_init = 0;
128+
param.upper_ring_id_init = 0;
129+
param.resolution = 1.0;
130+
param.intensity_pattern = {1, 1};
131+
param.positive_match_num_threshold = 1;
132+
param.negative_match_num_threshold = 1;
133+
param.match_intensity_difference_threshold = 1;
134+
param.vote_threshold_for_detect_marker = 0;
135+
param.marker_to_vehicle_offset_y = 123.0f;
136+
param.marker_height_from_ground = 2.0f;
137+
param.reference_ring_number = std::numeric_limits<uint8_t>::max();
138+
139+
// road_surface_mode OFF
140+
param.road_surface_mode = false;
141+
auto result_off = localizer->callDetectLandmarks<autoware::point_types::PointXYZIRC>(
142+
std::make_shared<sensor_msgs::msg::PointCloud2>(cloud));
143+
// OFF時はLandmarkのyがreference_ring_y（ダミーなのでmax値）
144+
if (!result_off.empty()) {
145+
EXPECT_EQ(result_off[0].pose.position.y, std::numeric_limits<float>::max());
146+
}
147+
148+
// road_surface_mode ON
149+
param.road_surface_mode = true;
150+
auto result_on = localizer->callDetectLandmarks<autoware::point_types::PointXYZIRC>(
151+
std::make_shared<sensor_msgs::msg::PointCloud2>(cloud));
152+
// ON時はLandmarkのyがmarker_to_vehicle_offset_y
153+
if (!result_on.empty()) {
154+
EXPECT_EQ(result_on[0].pose.position.y, param.marker_to_vehicle_offset_y);
155+
}
156+
}
157+
158+
} // namespace autoware::lidar_marker_localizer
159+
160+
int main(int argc, char ** argv)
161+
{
162+
rclcpp::init(argc, argv);
163+
::testing::InitGoogleTest(&argc, argv);
164+
int ret = RUN_ALL_TESTS();
165+
rclcpp::shutdown();
166+
return ret;
167+
}

0 commit comments

Comments
 (0)