Skip to content

Commit a3a4921

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 9e066b4 commit a3a4921

3 files changed

Lines changed: 186 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
@@ -69,6 +69,11 @@ class LidarMarkerLocalizer : public rclcpp::Node
6969
using SetBool = std_srvs::srv::SetBool;
7070
using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus;
7171

72+
public:
73+
explicit LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options);
74+
75+
protected:
76+
// For test subclass access
7277
struct Param
7378
{
7479
bool enable_read_all_target_ids;
@@ -109,8 +114,11 @@ class LidarMarkerLocalizer : public rclcpp::Node
109114
int64_t queue_size_for_debug_pub_msg;
110115
};
111116

112-
public:
113-
explicit LidarMarkerLocalizer(const rclcpp::NodeOptions & node_options);
117+
Param & mutable_param() { return param_; }
118+
const Param & param() const { return param_; }
119+
template <typename PointType>
120+
std::vector<landmark_manager::Landmark> detect_landmarks(
121+
const PointCloud2::ConstSharedPtr & points_msg_ptr);
114122

115123
private:
116124
void self_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & self_pose_msg_ptr);
@@ -121,9 +129,6 @@ class LidarMarkerLocalizer : public rclcpp::Node
121129

122130
void initialize_diagnostics();
123131
void main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr);
124-
template <typename PointType>
125-
std::vector<landmark_manager::Landmark> detect_landmarks(
126-
const PointCloud2::ConstSharedPtr & points_msg_ptr);
127132
sensor_msgs::msg::PointCloud2::SharedPtr extract_marker_pointcloud(
128133
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr,
129134
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: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,168 @@
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+
{"self_pose_timeout_sec", 1.0},
63+
{"self_pose_distance_tolerance_m", 1.0},
64+
{"limit_distance_from_self_pose_to_nearest_marker", 1.0},
65+
{"limit_distance_from_self_pose_to_nearest_marker_y", 1.0},
66+
{"limit_distance_from_self_pose_to_marker", 1.0},
67+
{"base_covariance", std::vector<double>(36, 0.0)},
68+
{"marker_width", 1.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+
param.marker_width = 1.0f;
139+
140+
// road_surface_mode OFF
141+
param.road_surface_mode = false;
142+
auto result_off = localizer->callDetectLandmarks<autoware::point_types::PointXYZIRC>(
143+
std::make_shared<sensor_msgs::msg::PointCloud2>(cloud));
144+
// OFF時はLandmarkのyがreference_ring_y（ダミーなのでmax値）
145+
if (!result_off.empty()) {
146+
EXPECT_EQ(result_off[0].pose.position.y, std::numeric_limits<float>::max());
147+
}
148+
149+
// road_surface_mode ON
150+
param.road_surface_mode = true;
151+
auto result_on = localizer->callDetectLandmarks<autoware::point_types::PointXYZIRC>(
152+
std::make_shared<sensor_msgs::msg::PointCloud2>(cloud));
153+
// ON時はLandmarkのyがmarker_to_vehicle_offset_y
154+
if (!result_on.empty()) {
155+
EXPECT_EQ(result_on[0].pose.position.y, param.marker_to_vehicle_offset_y);
156+
}
157+
}
158+
159+
} // namespace autoware::lidar_marker_localizer
160+
161+
int main(int argc, char ** argv)
162+
{
163+
rclcpp::init(argc, argv);
164+
::testing::InitGoogleTest(&argc, argv);
165+
int ret = RUN_ALL_TESTS();
166+
rclcpp::shutdown();
167+
return ret;
168+
}

0 commit comments

Comments
 (0)