Skip to content

Commit d9c9775

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 4939a62 commit d9c9775

3 files changed

Lines changed: 189 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: 171 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,171 @@
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+
#include <limits>
22+
#include <memory>
23+
#include <string>
24+
#include <vector>
25+
26+
namespace autoware::lidar_marker_localizer
27+
{
28+
29+
class TestableLidarMarkerLocalizer : public LidarMarkerLocalizer
30+
{
31+
public:
32+
using LidarMarkerLocalizer::LidarMarkerLocalizer;
33+
34+
template <typename PointT>
35+
auto callDetectLandmarks(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & points_msg_ptr)
36+
{
37+
return this->detect_landmarks<PointT>(points_msg_ptr);
38+
}
39+
40+
Param & public_mutable_param() { return mutable_param(); }
41+
const Param & public_param() const { return param(); }
42+
};
43+
44+
class LidarMarkerLocalizerTest : public ::testing::Test
45+
{
46+
protected:
47+
rclcpp::NodeOptions node_options;
48+
std::shared_ptr<TestableLidarMarkerLocalizer> localizer;
49+
50+
void SetUp() override
51+
{
52+
node_options.parameter_overrides(
53+
{{"enable_read_all_target_ids", false},
54+
{"target_ids", std::vector<std::string>{}},
55+
{"queue_size_for_output_pose", 1},
56+
{"marker_name", std::string("")},
57+
{"road_surface_mode", false},
58+
{"resolution", 1.0},
59+
{"intensity_pattern", std::vector<int64_t>{}},
60+
{"match_intensity_difference_threshold", 1},
61+
{"positive_match_num_threshold", 1},
62+
{"negative_match_num_threshold", 1},
63+
{"vote_threshold_for_detect_marker", 1},
64+
{"marker_to_vehicle_offset_y", 0.0},
65+
{"marker_height_from_ground", 0.0},
66+
{"reference_ring_number", 0},
67+
{"marker_width", 0.8},
68+
{"self_pose_timeout_sec", 1.0},
69+
{"self_pose_distance_tolerance_m", 1.0},
70+
{"limit_distance_from_self_pose_to_nearest_marker", 1.0},
71+
{"limit_distance_from_self_pose_to_nearest_marker_y", 1.0},
72+
{"limit_distance_from_self_pose_to_marker", 1.0},
73+
{"base_covariance", std::vector<double>(36, 0.0)},
74+
{"lower_ring_id_init", 0},
75+
{"upper_ring_id_init", 0},
76+
{"enable_save_log", false},
77+
{"save_file_directory_path", std::string("")},
78+
{"save_file_name", std::string("")},
79+
{"save_frame_id", std::string("")},
80+
{"radius_for_extracting_marker_pointcloud", 1.0},
81+
{"queue_size_for_debug_pub_msg", 1}});
82+
localizer = std::make_shared<TestableLidarMarkerLocalizer>(node_options);
83+
}
84+
}; // class LidarMarkerLocalizerTest : public ::testing::Test
85+
86+
TEST_F(LidarMarkerLocalizerTest, Initialization)
87+
{
88+
ASSERT_NE(localizer, nullptr);
89+
}
90+
91+
TEST_F(LidarMarkerLocalizerTest, DetectLandmarksRoadSurfaceModeSwitch)
92+
{
93+
// ダミーPointCloud2生成
94+
sensor_msgs::msg::PointCloud2 cloud;
95+
cloud.header.frame_id = "base_link";
96+
cloud.header.stamp.sec = 0;
97+
cloud.header.stamp.nanosec = 0;
98+
// 必要なフィールドをセット
99+
cloud.height = 1;
100+
cloud.width = 2;
101+
cloud.is_dense = true;
102+
cloud.is_bigendian = false;
103+
cloud.point_step =
104+
sizeof(float) * 4 + sizeof(uint16_t) + sizeof(float); // x, y, z, intensity, channel, dummy
105+
cloud.row_step = cloud.point_step * cloud.width;
106+
cloud.fields.resize(5);
107+
cloud.fields[0].name = "x";
108+
cloud.fields[0].offset = 0;
109+
cloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
110+
cloud.fields[0].count = 1;
111+
cloud.fields[1].name = "y";
112+
cloud.fields[1].offset = 4;
113+
cloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
114+
cloud.fields[1].count = 1;
115+
cloud.fields[2].name = "z";
116+
cloud.fields[2].offset = 8;
117+
cloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
118+
cloud.fields[2].count = 1;
119+
cloud.fields[3].name = "intensity";
120+
cloud.fields[3].offset = 12;
121+
cloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32;
122+
cloud.fields[3].count = 1;
123+
cloud.fields[4].name = "channel";
124+
cloud.fields[4].offset = 16;
125+
cloud.fields[4].datatype = sensor_msgs::msg::PointField::UINT16;
126+
cloud.fields[4].count = 1;
127+
cloud.data.resize(cloud.row_step * cloud.height);
128+
129+
// パラメータセット
130+
auto & param = localizer->public_mutable_param();
131+
param.lower_ring_id_init = 0;
132+
param.upper_ring_id_init = 0;
133+
param.resolution = 1.0;
134+
param.intensity_pattern = {1, 1};
135+
param.positive_match_num_threshold = 1;
136+
param.negative_match_num_threshold = 1;
137+
param.match_intensity_difference_threshold = 1;
138+
param.vote_threshold_for_detect_marker = 0;
139+
param.marker_to_vehicle_offset_y = 123.0f;
140+
param.marker_height_from_ground = 2.0f;
141+
param.reference_ring_number = std::numeric_limits<uint8_t>::max();
142+
143+
// road_surface_mode OFF
144+
param.road_surface_mode = false;
145+
auto result_off = localizer->callDetectLandmarks<autoware::point_types::PointXYZIRC>(
146+
std::make_shared<sensor_msgs::msg::PointCloud2>(cloud));
147+
// OFF時はLandmarkのyがreference_ring_y（ダミーなのでmax値）
148+
if (!result_off.empty()) {
149+
EXPECT_EQ(result_off[0].pose.position.y, std::numeric_limits<float>::max());
150+
}
151+
152+
// road_surface_mode ON
153+
param.road_surface_mode = true;
154+
auto result_on = localizer->callDetectLandmarks<autoware::point_types::PointXYZIRC>(
155+
std::make_shared<sensor_msgs::msg::PointCloud2>(cloud));
156+
// ON時はLandmarkのyがmarker_to_vehicle_offset_y
157+
if (!result_on.empty()) {
158+
EXPECT_EQ(result_on[0].pose.position.y, param.marker_to_vehicle_offset_y);
159+
}
160+
}
161+
162+
} // namespace autoware::lidar_marker_localizer
163+
164+
int main(int argc, char ** argv)
165+
{
166+
rclcpp::init(argc, argv);
167+
::testing::InitGoogleTest(&argc, argv);
168+
int ret = RUN_ALL_TESTS();
169+
rclcpp::shutdown();
170+
return ret;
171+
}

0 commit comments

Comments
 (0)